├── .circleci └── config.yml ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── roverrobotics_description ├── CHANGELOG.rst ├── CMakeLists.txt ├── launch │ ├── description-2wd.launch │ └── description-4wd.launch ├── meshes │ ├── 2WD_Rover │ │ ├── 2WDCore.stl │ │ ├── LeftFrontTire.stl │ │ ├── LeftRearCastorHousing.stl │ │ ├── LeftRearCastorWheel.stl │ │ ├── RightFrontTire.stl │ │ ├── RightRearCastorHousing.stl │ │ └── RightRearCastorWheel.stl │ ├── 4WD_Rover │ │ ├── 4WDCore.stl │ │ ├── LeftFrontWheel.stl │ │ ├── LeftRearWheel.stl │ │ ├── RightFrontWheel.stl │ │ └── RightRearWheel.stl │ └── hokuyo.dae ├── package.xml ├── scripts │ └── env_run └── urdf │ ├── 2WD_Rover │ ├── 2WD_rover.gazebo.xacro │ └── 2WD_rover.urdf.xacro │ ├── 4WD_Rover │ ├── 4WD_rover.gazebo.xacro │ └── 4WD_rover.urdf.xacro │ ├── accessories │ ├── SLAM_pack.urdf.xacro │ └── accessories.urdf.xacro │ ├── common │ ├── common_properties.xacro │ └── materials.xacro │ └── configs │ ├── 4WD_Rover │ └── 4WD_Rover_with_SLAM_Pack ├── roverrobotics_driver ├── CMakeLists.txt ├── config │ ├── twist_mux_locks.yaml │ └── twist_mux_topics_locks.yaml ├── docs │ ├── Function Diagram.png │ ├── Ros Stack (2).png │ ├── RoverProProtocol_Implementation.png │ └── source │ │ └── Function Diagram.xml ├── include │ └── roverrobotics_ros_driver.hpp ├── launch │ ├── mfg │ │ ├── mfg_mini.launch │ │ ├── mfg_pro2.launch │ │ ├── mfg_pro_2WD.launch │ │ ├── mfg_pro_4WD.launch │ │ ├── mfg_zero2_2WD.launch │ │ └── mfg_zero2_4WD.launch │ ├── mini.launch │ ├── mini_teleop.launch │ ├── miti.launch │ ├── miti_teleop.launch │ ├── pro.launch │ ├── pro2.launch │ ├── pro2_teleop.launch │ ├── pro_teleop.launch │ ├── zero2.launch │ └── zero2_teleop.launch ├── package.xml └── src │ └── roverrobotics_ros_driver.cpp ├── roverrobotics_input_manager ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── config │ ├── controller_config.yaml │ ├── input_topics.yaml │ └── test.yaml ├── launch │ └── example.launch ├── package.xml ├── scripts │ ├── control_input_manager.py │ └── ps4_mapper.py └── test │ ├── control_input_manager.test │ ├── test_functional.py │ └── test_latency.py └── roverrobotics_simulation ├── CHANGELOG.rst ├── CMakeLists.txt ├── launch ├── gazebo │ ├── 2wd_rover_gazebo.launch │ ├── 2wd_rover_gazebo_teleop.launch │ ├── 4wd_rover_gazebo.launch │ └── 4wd_rover_gazebo_teleop.launch └── rviz │ ├── 2wd_rover_rviz.launch │ └── 4wd_rover_rviz.launch └── package.xml /.circleci/config.yml: -------------------------------------------------------------------------------- 1 | jobs: 2 | build-noetic: 3 | working_directory: /catkin_ws/src 4 | docker: 5 | - image: ros:noetic-ros-core-focal 6 | steps: 7 | - checkout 8 | - run: 9 | command: | 10 | apt-get update 11 | apt-get install -y build-essential 12 | apt-get install -y ros-noetic-tf 13 | - run: 14 | command: | 15 | source /opt/ros/noetic/setup.bash 16 | catkin_init_workspace 17 | cd .. 18 | catkin_make run_tests 19 | catkin_test_results 20 | # Exit code 1 to cause build step failure 21 | if [ $(catkin_test_results | awk '/Summary/ {print $6}') != 0 ]; then exit 1; fi 22 | 23 | workflows: 24 | version: 2 25 | build-and-test: 26 | jobs: 27 | - build-noetic: 28 | filters: 29 | branches: 30 | only: 31 | - master 32 | - develop 33 | - noetic-devel 34 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build/* 2 | .vscode/* 3 | workspace.code-workspace 4 | 5 | 6 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/noetic/share/catkin/cmake/toplevel.cmake 2 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Roverrobotics_ros1 2 | ## About: 3 | - This is a ROS wrapper to interface with roverrobotics' robots 4 | - Librover is required in order to use this wrapper 5 | 6 | ## Installation instructions 7 | 8 | 1. Cloning this repository into your workspace 9 | ``` 10 | cd workspace/src/ 11 | git clone https://github.com/RoverRobotics/roverrobotics_ros1 12 | ``` 13 | 2. Install Udev rules for robot 14 | ``` 15 | cd workspace/src/roverrobotics_driver/udev 16 | sudo cp 55-roverrobotics.rules /etc/udev/rules.d/55-roverrobotics.rules && sudo udevadm control --reload-rules && udevadm trigger 17 | ``` 18 | 3. Install shared library 19 | ``` 20 | cd ~/ 21 | mkdir library/ 22 | cd library/ 23 | git clone https://github.com/RoverRobotics/librover 24 | cd librover/ 25 | cmake . 26 | make 27 | sudo make install 28 | ``` 29 | 4. Rebuild your workspace 30 | ``` 31 | cd workspace/ 32 | catkin_make 33 | ``` 34 | 5. Update auto complete 35 | ``` 36 | source devels/setup.bash 37 | ``` 38 | 6. Launch Robot (replace with your robot config.) 39 | ``` 40 | roslaunch roverrobotics_driver pro.launch 41 | ``` 42 | -------------------------------------------------------------------------------- /roverrobotics_description/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package roverrobotics_description 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 2.0.1 (2020-12-14) 5 | ------------------ 6 | 7 | 2.0.0 (2020-11-10) 8 | ------------------ 9 | 10 | 1.2.0 (2020-11-05) 11 | ------------------ 12 | 13 | 1.1.1 (2020-06-15) 14 | ------------------ 15 | * Flip orrientation of Lidar, and rename launch file names 16 | * add pyserial to package.xml 17 | * Contributors: 1102, padiln 18 | 19 | 1.1.0 (2020-06-08) 20 | ------------------ 21 | * release to melodic 22 | 23 | 1.0.0 (2020-06-08) 24 | ------------------ 25 | * Merge branch 'develop' into feature/padiln/rviz_gazebo_sim 26 | * adding rviz and gazebo simulation to rr_openrover stack 27 | * added support for gazebo and rviz simulation for 2wd and 4wd rovers 28 | * Contributors: padiln 29 | 30 | 0.8.0 (2020-03-25) 31 | ------------------ 32 | 33 | 0.7.4 (2020-02-19) 34 | ------------------ 35 | * change Maintainership 36 | * Adding description and 4WD code 37 | -------------------------------------------------------------------------------- /roverrobotics_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(roverrobotics_description) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | rospy 13 | std_msgs 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a exec_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | # generate_messages( 72 | # DEPENDENCIES 73 | # std_msgs 74 | # ) 75 | 76 | ################################################ 77 | ## Declare ROS dynamic reconfigure parameters ## 78 | ################################################ 79 | 80 | ## To declare and build dynamic reconfigure parameters within this 81 | ## package, follow these steps: 82 | ## * In the file package.xml: 83 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 84 | ## * In this file (CMakeLists.txt): 85 | ## * add "dynamic_reconfigure" to 86 | ## find_package(catkin REQUIRED COMPONENTS ...) 87 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 88 | ## and list every .cfg file to be processed 89 | 90 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 91 | # generate_dynamic_reconfigure_options( 92 | # cfg/DynReconf1.cfg 93 | # cfg/DynReconf2.cfg 94 | # ) 95 | 96 | ################################### 97 | ## catkin specific configuration ## 98 | ################################### 99 | ## The catkin_package macro generates cmake config files for your package 100 | ## Declare things to be passed to dependent projects 101 | ## INCLUDE_DIRS: uncomment this if your package contains header files 102 | ## LIBRARIES: libraries you create in this project that dependent projects also need 103 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 104 | ## DEPENDS: system dependencies of this project that dependent projects also need 105 | catkin_package( 106 | # INCLUDE_DIRS include 107 | # LIBRARIES roverrobotics_description 108 | # CATKIN_DEPENDS roscpp rospy std_msgs 109 | # DEPENDS system_lib 110 | ) 111 | 112 | ########### 113 | ## Build ## 114 | ########### 115 | 116 | ## Specify additional locations of header files 117 | ## Your package locations should be listed before other locations 118 | include_directories( 119 | # include 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/roverrobotics_description.cpp 126 | # ) 127 | 128 | ## Add cmake target dependencies of the library 129 | ## as an example, code may need to be generated before libraries 130 | ## either from message generation or dynamic reconfigure 131 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 132 | 133 | ## Declare a C++ executable 134 | ## With catkin_make all packages are built within a single CMake context 135 | ## The recommended prefix ensures that target names across packages don't collide 136 | # add_executable(${PROJECT_NAME}_node src/roverrobotics_description_node.cpp) 137 | 138 | ## Rename C++ executable without prefix 139 | ## The above recommended prefix causes long target names, the following renames the 140 | ## target back to the shorter version for ease of user use 141 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 142 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 143 | 144 | ## Add cmake target dependencies of the executable 145 | ## same as for the library above 146 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 147 | 148 | ## Specify libraries to link a library or executable target against 149 | # target_link_libraries(${PROJECT_NAME}_node 150 | # ${catkin_LIBRARIES} 151 | # ) 152 | 153 | ############# 154 | ## Install ## 155 | ############# 156 | 157 | # all install targets should use catkin DESTINATION variables 158 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 159 | 160 | install(DIRECTORY launch/ 161 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 162 | FILES_MATCHING PATTERN "*.launch" 163 | ) 164 | 165 | install(DIRECTORY meshes/ 166 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes 167 | FILES_MATCHING PATTERN "*" 168 | ) 169 | 170 | install(DIRECTORY urdf/ 171 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf 172 | FILES_MATCHING PATTERN "*" 173 | ) 174 | install(PROGRAMS scripts/env_run 175 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/scripts 176 | ) 177 | ############# 178 | ## Testing ## 179 | ############# 180 | 181 | ## Add gtest based cpp test target and link libraries 182 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_roverrobotics_description.cpp) 183 | # if(TARGET ${PROJECT_NAME}-test) 184 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 185 | # endif() 186 | 187 | ## Add folders to be run by python nosetests 188 | # catkin_add_nosetests(test) 189 | -------------------------------------------------------------------------------- /roverrobotics_description/launch/description-2wd.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /roverrobotics_description/launch/description-4wd.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /roverrobotics_description/meshes/2WD_Rover/2WDCore.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoverRobotics/roverrobotics_ros1/5113517978e7393ae597d259d67b78386f0c0869/roverrobotics_description/meshes/2WD_Rover/2WDCore.stl -------------------------------------------------------------------------------- /roverrobotics_description/meshes/2WD_Rover/LeftFrontTire.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoverRobotics/roverrobotics_ros1/5113517978e7393ae597d259d67b78386f0c0869/roverrobotics_description/meshes/2WD_Rover/LeftFrontTire.stl -------------------------------------------------------------------------------- /roverrobotics_description/meshes/2WD_Rover/LeftRearCastorHousing.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoverRobotics/roverrobotics_ros1/5113517978e7393ae597d259d67b78386f0c0869/roverrobotics_description/meshes/2WD_Rover/LeftRearCastorHousing.stl -------------------------------------------------------------------------------- /roverrobotics_description/meshes/2WD_Rover/LeftRearCastorWheel.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoverRobotics/roverrobotics_ros1/5113517978e7393ae597d259d67b78386f0c0869/roverrobotics_description/meshes/2WD_Rover/LeftRearCastorWheel.stl -------------------------------------------------------------------------------- /roverrobotics_description/meshes/2WD_Rover/RightFrontTire.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoverRobotics/roverrobotics_ros1/5113517978e7393ae597d259d67b78386f0c0869/roverrobotics_description/meshes/2WD_Rover/RightFrontTire.stl -------------------------------------------------------------------------------- /roverrobotics_description/meshes/2WD_Rover/RightRearCastorHousing.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoverRobotics/roverrobotics_ros1/5113517978e7393ae597d259d67b78386f0c0869/roverrobotics_description/meshes/2WD_Rover/RightRearCastorHousing.stl -------------------------------------------------------------------------------- /roverrobotics_description/meshes/2WD_Rover/RightRearCastorWheel.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoverRobotics/roverrobotics_ros1/5113517978e7393ae597d259d67b78386f0c0869/roverrobotics_description/meshes/2WD_Rover/RightRearCastorWheel.stl -------------------------------------------------------------------------------- /roverrobotics_description/meshes/4WD_Rover/4WDCore.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoverRobotics/roverrobotics_ros1/5113517978e7393ae597d259d67b78386f0c0869/roverrobotics_description/meshes/4WD_Rover/4WDCore.stl -------------------------------------------------------------------------------- /roverrobotics_description/meshes/4WD_Rover/LeftFrontWheel.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoverRobotics/roverrobotics_ros1/5113517978e7393ae597d259d67b78386f0c0869/roverrobotics_description/meshes/4WD_Rover/LeftFrontWheel.stl -------------------------------------------------------------------------------- /roverrobotics_description/meshes/4WD_Rover/LeftRearWheel.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoverRobotics/roverrobotics_ros1/5113517978e7393ae597d259d67b78386f0c0869/roverrobotics_description/meshes/4WD_Rover/LeftRearWheel.stl -------------------------------------------------------------------------------- /roverrobotics_description/meshes/4WD_Rover/RightFrontWheel.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoverRobotics/roverrobotics_ros1/5113517978e7393ae597d259d67b78386f0c0869/roverrobotics_description/meshes/4WD_Rover/RightFrontWheel.stl -------------------------------------------------------------------------------- /roverrobotics_description/meshes/4WD_Rover/RightRearWheel.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoverRobotics/roverrobotics_ros1/5113517978e7393ae597d259d67b78386f0c0869/roverrobotics_description/meshes/4WD_Rover/RightRearWheel.stl -------------------------------------------------------------------------------- /roverrobotics_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | roverrobotics_description 4 | 2.0.1 5 | The roverrobotics_description package 6 | 7 | 8 | 9 | 10 | John Bartholomew 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | catkin 19 | roscpp 20 | rospy 21 | std_msgs 22 | roscpp 23 | rospy 24 | std_msgs 25 | roscpp 26 | rospy 27 | std_msgs 28 | gazebo_msgs 29 | gazebo_plugins 30 | gazebo_ros 31 | gazebo_ros_control 32 | gazebo_ros_pkgs 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /roverrobotics_description/scripts/env_run: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # This simple wrapper allowing us to pass a set of 3 | # environment variables to be sourced prior to running 4 | # another command. Used in the launch file for setting 5 | # robot configurations prior to xacro. 6 | # credit for this file goes to Mike Purvis 7 | 8 | ENVVARS_FILE=$1 9 | shift 1 10 | 11 | set -a 12 | source $ENVVARS_FILE 13 | $@ 14 | -------------------------------------------------------------------------------- /roverrobotics_description/urdf/2WD_Rover/2WD_rover.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Gazebo/FlatBlack 6 | 7 | 8 | 9 | 1.0 10 | 1.0 11 | 300000.0 12 | Gazebo/Black 13 | 14 | 15 | 16 | 1.0 17 | 1.0 18 | 300000.0 19 | Gazebo/Black 20 | 21 | 22 | 23 | Gazebo/Grey 24 | 25 | 26 | 27 | 0.6 28 | 0.6 29 | 200000.0 30 | Gazebo/Grey 31 | 32 | 33 | 34 | Gazebo/Grey 35 | 36 | 37 | 38 | 0.6 39 | 0.6 40 | 200000.0 41 | Gazebo/Grey 42 | 43 | 44 | 45 | 46 | / 47 | odom 48 | odom 49 | cmd_vel/managed 50 | true 51 | false 52 | base_footprint 53 | false 54 | 15.0 55 | left_front_wheel_joint 56 | right_front_wheel_joint 57 | 0.358 58 | 0.254 59 | 0.7 60 | 8 61 | false 62 | 63 | odom:=odom_raw 64 | 65 | 66 | 67 | 68 | 69 | 70 | / 71 | 72 | left_front_wheel_joint, 73 | right_front_wheel_joint, 74 | left_rear_castor_housing_joint, 75 | left_rear_castor_wheel_joint, 76 | right_rear_castor_housing_joint, 77 | right_rear_castor_wheel_joint 78 | 79 | true 80 | 30.0 81 | 82 | 83 | 84 | -------------------------------------------------------------------------------- /roverrobotics_description/urdf/2WD_Rover/2WD_rover.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 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 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | ps -aux | grep gazebo | awk $2 112 | 113 | 114 | 115 | 116 | 117 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | -------------------------------------------------------------------------------- /roverrobotics_description/urdf/4WD_Rover/4WD_rover.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Gazebo/FlatBlack 6 | 7 | 8 | 9 | 1.0 10 | 1.0 11 | 300000.0 12 | Gazebo/Black 13 | 14 | 15 | 16 | 1.0 17 | 1.0 18 | 300000.0 19 | Gazebo/Black 20 | 21 | 22 | 23 | 1.0 24 | 1.0 25 | 300000.0 26 | Gazebo/Black 27 | 28 | 29 | 30 | 1.0 31 | 1.0 32 | 300000.0 33 | Gazebo/Black 34 | 35 | 36 | 37 | 38 | 15.0 39 | / 40 | left_front_wheel_joint 41 | right_front_wheel_joint 42 | left_rear_wheel_joint 43 | right_rear_wheel_joint 44 | 0.358 45 | 0.254 46 | base_footprint 47 | 8 48 | cmd_vel/managed 49 | false 50 | 51 | 52 | 53 | 54 | 55 | / 56 | 57 | left_front_wheel_joint, 58 | right_front_wheel_joint, 59 | left_rear_wheel_joint, 60 | right_rear_wheel_joint 61 | 62 | 30.0 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /roverrobotics_description/urdf/4WD_Rover/4WD_rover.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 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 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | -------------------------------------------------------------------------------- /roverrobotics_description/urdf/accessories/SLAM_pack.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 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 | true 92 | 30.0 93 | depth_image_link 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 12.0 124 | 125 | scan 126 | 127 | sensor_msgs/LaserScan 128 | lidar_link 129 | 130 | 131 | 132 | 133 | 360 134 | 1 135 | -3.14159 136 | 3.14159 137 | 138 | 139 | 140 | 0.120 141 | 20 142 | 0.015 143 | 144 | 145 | gaussian 146 | 0.0 147 | 0.01 148 | 149 | 150 | 151 | Gazebo/DarkGrey 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 30.0 181 | /imu 182 | imu_link 183 | 184 | 185 | Gazebo/Blue 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | -------------------------------------------------------------------------------- /roverrobotics_description/urdf/accessories/accessories.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /roverrobotics_description/urdf/common/common_properties.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /roverrobotics_description/urdf/common/materials.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /roverrobotics_description/urdf/configs/4WD_Rover: -------------------------------------------------------------------------------- 1 | DRIVE_TYPE=1 2 | -------------------------------------------------------------------------------- /roverrobotics_description/urdf/configs/4WD_Rover_with_SLAM_Pack: -------------------------------------------------------------------------------- 1 | SLAM_PACK=1 2 | -------------------------------------------------------------------------------- /roverrobotics_driver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8.0) 2 | project(roverrobotics_driver) 3 | 4 | add_compile_options(-std=c++17) 5 | find_package(catkin REQUIRED COMPONENTS 6 | roscpp 7 | rospy 8 | std_msgs 9 | message_generation 10 | geometry_msgs 11 | sensor_msgs 12 | nav_msgs 13 | tf2 14 | pluginlib 15 | nav_msgs 16 | tf2_geometry_msgs 17 | ) 18 | 19 | catkin_package( 20 | INCLUDE_DIRS 21 | CATKIN_DEPENDS 22 | message_runtime 23 | pluginlib 24 | roscpp 25 | std_msgs 26 | sensor_msgs 27 | ds4_driver 28 | nav_msgs 29 | ) 30 | 31 | ########### 32 | ## Build ## 33 | ########### 34 | 35 | include_directories( 36 | include 37 | ${catkin_INCLUDE_DIRS} 38 | ) 39 | 40 | add_library(librover SHARED IMPORTED) 41 | set_target_properties(librover PROPERTIES 42 | IMPORTED_LOCATION "/usr/lib/liblibrover.so" 43 | INTERFACE_INCLUDE_DIRECTORIES "/usr/include/librover") 44 | add_executable(roverrobotics_driver_node src/roverrobotics_ros_driver.cpp) 45 | ##add_dependencies(roverrobotics_driver_node roverrobotics_driver_generate_messages_cpp ${catkin_EXPORTED_TARGETS}) 46 | 47 | 48 | target_link_libraries(roverrobotics_driver_node ${catkin_LIBRARIES} librover) 49 | 50 | ############# 51 | ## Install ## 52 | ############# 53 | 54 | ## Mark executables and/or libraries for installation 55 | install(TARGETS roverrobotics_driver_node 56 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 57 | ) 58 | 59 | # ##Install Linked Library 60 | # install(TARGETS librover 61 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 62 | # ) 63 | 64 | ## Copy launch files 65 | install(DIRECTORY launch/ 66 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 67 | FILES_MATCHING PATTERN "*.launch" 68 | ) 69 | 70 | ## Copy config files 71 | install(DIRECTORY config/ 72 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config 73 | FILES_MATCHING PATTERN "*.yaml" 74 | ) 75 | 76 | ## Copy header files 77 | install(DIRECTORY include/${PROJECT_NAME}/ 78 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 79 | FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" 80 | ) 81 | 82 | ## Copy any python scripts 83 | ##catkin_install_python(PROGRAMS scripts/diagnostics.py 84 | ## DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ 85 | ##) 86 | 87 | ############# 88 | ## Testing ## 89 | ############# 90 | 91 | # TODO 92 | -------------------------------------------------------------------------------- /roverrobotics_driver/config/twist_mux_locks.yaml: -------------------------------------------------------------------------------- 1 | # Locks to stop the twist inputs. 2 | # For each lock: 3 | # - topic : input topic that provides the lock; it must be of type std_msgs::Bool?!!! 4 | # - timeout : == 0.0 -> not used 5 | # > 0.0 -> the lock is supposed to published at a certain frequency in order 6 | # to detect that the publisher is alive; the timeout in seconds allows 7 | # to detect that, and if the publisher dies we will enable the lock 8 | # - priority: priority in the range [0, 255], so all the topics with priority lower than it 9 | # will be stopped/disabled 10 | 11 | locks: 12 | - 13 | name : navigation 14 | topic : pause_navigation 15 | timeout : 0.0 16 | # Same priority as keyboard control, so it'll not block it. 17 | priority: 90 18 | - 19 | name : joystick 20 | topic : joy_priority 21 | timeout : 0.0 22 | priority: 100 23 | 24 | -------------------------------------------------------------------------------- /roverrobotics_driver/config/twist_mux_topics_locks.yaml: -------------------------------------------------------------------------------- 1 | # Input topics handled/muxed. 2 | # For each topic: 3 | # - name : name identifier to select the topic 4 | # - topic : input topic of geometry_msgs::Twist type 5 | # - timeout : timeout in seconds to start discarding old messages, and use 0.0 speed instead 6 | # - priority: priority in the range [0, 255]; the higher the more priority over other topics 7 | 8 | topics: 9 | - 10 | name : navigation 11 | topic : /cmd_vel 12 | timeout : 0.5 13 | priority: 10 14 | - 15 | name : keyboard 16 | topic : /managed/key 17 | timeout : 0.5 18 | priority: 90 19 | - 20 | name : joystick 21 | topic : /managed/joy 22 | timeout : 0.5 23 | priority: 100 24 | - 25 | name : autodock 26 | topic : /managed/auto_dock 27 | timeout : 0.5 28 | priority: 10 29 | - 30 | name : freedom 31 | topic : /managed/freedom 32 | timeout : 0.5 33 | priority: 90 34 | -------------------------------------------------------------------------------- /roverrobotics_driver/docs/Function Diagram.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoverRobotics/roverrobotics_ros1/5113517978e7393ae597d259d67b78386f0c0869/roverrobotics_driver/docs/Function Diagram.png -------------------------------------------------------------------------------- /roverrobotics_driver/docs/Ros Stack (2).png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoverRobotics/roverrobotics_ros1/5113517978e7393ae597d259d67b78386f0c0869/roverrobotics_driver/docs/Ros Stack (2).png -------------------------------------------------------------------------------- /roverrobotics_driver/docs/RoverProProtocol_Implementation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoverRobotics/roverrobotics_ros1/5113517978e7393ae597d259d67b78386f0c0869/roverrobotics_driver/docs/RoverProProtocol_Implementation.png -------------------------------------------------------------------------------- /roverrobotics_driver/docs/source/Function Diagram.xml: -------------------------------------------------------------------------------- 1 | 2 | 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 3 | -------------------------------------------------------------------------------- /roverrobotics_driver/include/roverrobotics_ros_driver.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | #include "geometry_msgs/Twist.h" 7 | #include "librover/control.hpp" 8 | #include "librover/protocol_mini.hpp" 9 | #include "librover/protocol_mega.hpp" 10 | #include "librover/protocol_zero_2.hpp" 11 | #include "librover/protocol_pro.hpp" 12 | #include "librover/protocol_pro_2.hpp" 13 | #include "nav_msgs/Odometry.h" 14 | #include "ros/node_handle.h" 15 | #include "ros/ros.h" 16 | #include "sensor_msgs/JoyFeedbackArray.h" 17 | #include "std_msgs/Bool.h" 18 | #include "std_msgs/Float32.h" 19 | #include "std_msgs/Float32MultiArray.h" 20 | #include "tf2_geometry_msgs/tf2_geometry_msgs.h" 21 | #include "time.h" 22 | 23 | namespace RoverRobotics { 24 | 25 | class RobotDriver { 26 | private: 27 | std::unique_ptr robot_; 28 | // Pub Sub 29 | ros::Subscriber speed_command_subscriber_; // listen to cmd_vel inputs 30 | ros::Subscriber mode_trigger_subscriber_; // listen to change mode inputs 31 | ros::Subscriber trim_command_subscriber_; // listen to trim value broadcast 32 | ros::Subscriber estop_trigger_subscriber_; // listen to estop trigger inputs 33 | ros::Subscriber estop_reset_subscriber_; // listen to estop reset inputs 34 | 35 | ros::Subscriber robot_info_subscriber_; // listen to robot_info request 36 | ros::Publisher robot_info_publisher_; // publish robot_unique info 37 | 38 | ros::Publisher robot_status_publisher_; // publish robot state (battery, 39 | // estop_status, speed) 40 | ros::Publisher feedback_publisher_; 41 | ros::Publisher robot_odom_publisher_; // publish odometry 42 | // parameter variables 43 | std::string speed_topic_; 44 | std::string estop_trigger_topic_; 45 | std::string mode_trigger_topic_; 46 | std::string estop_reset_topic_; 47 | std::string robot_status_topic_; 48 | std::string odom_frame_id_; 49 | std::string odom_child_frame_id_; 50 | float robot_status_frequency_; 51 | float robot_odom_frequency_; 52 | std::string robot_info_request_topic_; 53 | std::string robot_info_topic_; 54 | std::string robot_type_; 55 | std::string trim_topic_; 56 | std::string mode_name_; 57 | float odom_angular_coef_; 58 | float odom_traction_factor_; 59 | float trimvalue_; 60 | std::string device_port_; 61 | std::string comm_type_; 62 | // Timer 63 | ros::Timer robot_status_timer_; 64 | ros::Timer odom_publish_timer_; 65 | 66 | bool estop_state_ = false; 67 | Control::pid_gains pidGains_ = {0, 0, 0}; 68 | Control::robot_motion_mode_t robot_mode_; 69 | Control::angular_scaling_params angular_scaling_params_ = {0, 0, 0, 0, 0}; 70 | const float PID_P_DEFAULT_ = 0; 71 | const float PID_I_DEFAULT_ = 0; 72 | const float PID_D_DEFAULT_ = 0; 73 | const float PID_P_MAX_ = 1; 74 | const float PID_P_MIN_ = 0; 75 | const float PID_I_MAX_ = 1; 76 | const float PID_I_MIN_ = 0; 77 | const float PID_D_MAX_ = 1; 78 | const float PID_D_MIN_ = 0; 79 | const float ROBOT_STATUS_FREQUENCY_DEFAULT_ = 5; 80 | const float ROBOT_STATUS_FREQUENCY_MAX_ = 60; 81 | const float ROBOT_STATUS_FREQUENCY_MIN_ = 5; 82 | const float ROBOT_ODOM_FREQUENCY_DEFAULT_ = 30.00; 83 | const float ODOM_ANGULAR_COEF_DEFAULT_ = 0; 84 | const float ODOM_TRCTION_FACTOR_DEFAULT_ = 0; 85 | const std::string ODOM_FRAME_ID_DEFAULT_ = "odom"; 86 | const std::string ODOM_CHILD_FRAME_ID_DEFAULT_ = "base_link"; 87 | const float ANGULAR_SCALING_A_DEFAULT_ = 0; 88 | const float ANGULAR_SCALING_B_DEFAULT_ = 0; 89 | const float ANGULAR_SCALING_C_DEFAULT_ = 0; 90 | const float ANGULAR_SCALING_MIN_DEFAULT_ = 1; 91 | const float ANGULAR_SCALING_MAX_DEFAULT_ = 1; 92 | 93 | public: 94 | RobotDriver(ros::NodeHandle *nh); 95 | ~RobotDriver(); 96 | void publishRobotStatus(const ros::TimerEvent &event); 97 | void publishOdometry(const ros::TimerEvent &event); 98 | void publishRobotInfo(); 99 | void callbackModeTrigger(const std_msgs::Bool::ConstPtr &msg); 100 | void callbackSpeedCommand(const geometry_msgs::Twist &msg); 101 | void callbackTrim(const std_msgs::Float32::ConstPtr &msg); 102 | void callbackEstopTrigger(const std_msgs::Bool::ConstPtr &msg); 103 | void callbackInfo(const std_msgs::Bool::ConstPtr &msg); 104 | void callbackEstopReset(const std_msgs::Bool::ConstPtr &msg); 105 | }; 106 | } // namespace RoverRobotics 107 | -------------------------------------------------------------------------------- /roverrobotics_driver/launch/mfg/mfg_mini.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /roverrobotics_driver/launch/mfg/mfg_pro2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /roverrobotics_driver/launch/mfg/mfg_pro_2WD.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /roverrobotics_driver/launch/mfg/mfg_pro_4WD.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /roverrobotics_driver/launch/mfg/mfg_zero2_2WD.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /roverrobotics_driver/launch/mfg/mfg_zero2_4WD.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /roverrobotics_driver/launch/mini.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /roverrobotics_driver/launch/mini_teleop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /roverrobotics_driver/launch/miti.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /roverrobotics_driver/launch/miti_teleop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /roverrobotics_driver/launch/pro.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /roverrobotics_driver/launch/pro2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /roverrobotics_driver/launch/pro2_teleop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /roverrobotics_driver/launch/pro_teleop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /roverrobotics_driver/launch/zero2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /roverrobotics_driver/launch/zero2_teleop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /roverrobotics_driver/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | roverrobotics_driver 4 | 1.0.0 5 | 6 | Rover Robotics Driver Ros 1 7 | 8 | 9 | 10 | 11 | 12 | John Bartholomew 13 | William Rook 14 | BSD 15 | 16 | catkin 17 | roscpp 18 | rospy 19 | std_msgs 20 | sensor_msgs 21 | tf2 22 | tf2_geometry_msgs 23 | pluginlib 24 | roscpp 25 | rospy 26 | std_msgs 27 | roscpp 28 | rospy 29 | std_msgs 30 | ds4_driver 31 | message_runtime 32 | pluginlib 33 | sensor_msgs 34 | rostest 35 | nav_msgs 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /roverrobotics_driver/src/roverrobotics_ros_driver.cpp: -------------------------------------------------------------------------------- 1 | #include "roverrobotics_ros_driver.hpp" 2 | namespace RoverRobotics { 3 | 4 | RobotDriver::RobotDriver(ros::NodeHandle *nh) { 5 | ROS_INFO("Starting Rover Driver Node"); 6 | // Robot Parameters 7 | estop_state_ = false; 8 | if (!ros::param::get("device_port", device_port_)) { 9 | ROS_FATAL("No 'device_port' set, Shutting down Driver Node"); 10 | ros::shutdown(); 11 | } 12 | if (!ros::param::get("comm_type", comm_type_)) { 13 | ROS_FATAL("No communication method set, Shutting down Driver Node"); 14 | ros::shutdown(); 15 | } 16 | mode_name_ = "OPEN_LOOP"; 17 | if (!ros::param::get("robot_mode", mode_name_)) { 18 | ROS_INFO("no 'robot_mode' set; using the default value: 'OPEN_LOOP'"); 19 | } 20 | 21 | if (mode_name_ == "TRACTION_CONTROL") 22 | robot_mode_ = Control::TRACTION_CONTROL; 23 | else if (mode_name_ == "INDEPENDENT_WHEEL") 24 | robot_mode_ = Control::INDEPENDENT_WHEEL; 25 | else 26 | robot_mode_ = Control::OPEN_LOOP; 27 | // PID Control 28 | if (robot_mode_ != Control::OPEN_LOOP) { 29 | ROS_WARN( 30 | "Closed Loop Control is ACTIVE. Please make sure your PID is properly " 31 | "tuned"); 32 | } 33 | if (!ros::param::get("Kp", pidGains_.kp)) { 34 | pidGains_.kp = PID_P_DEFAULT_; 35 | ROS_INFO("no 'Kp' set; using the default value: %f", pidGains_.kp); 36 | } 37 | if (pidGains_.kp < PID_P_MIN_) { 38 | ROS_WARN("pidGains_.Kp is too low, changing to: %f", PID_P_MIN_); 39 | pidGains_.kp = PID_P_MIN_; 40 | } else if (pidGains_.kp > PID_P_MAX_) { 41 | ROS_WARN("pidGains_.Kp is too high, changing to: %f", PID_P_MAX_); 42 | pidGains_.kp = PID_P_MAX_; 43 | } 44 | if (!ros::param::get("Ki", pidGains_.ki)) { 45 | pidGains_.ki = PID_I_DEFAULT_; 46 | ROS_INFO("no 'Ki' set; using the default value: %f", PID_I_DEFAULT_); 47 | } 48 | if (pidGains_.ki < PID_I_MIN_) { 49 | ROS_WARN("pidGains_.Ki is too low, changing to: %f", PID_I_MIN_); 50 | pidGains_.ki = PID_I_MIN_; 51 | } else if (pidGains_.ki > PID_I_MAX_) { 52 | ROS_WARN("pidGains_.Ki is too high, changing to: %f", PID_I_MAX_); 53 | pidGains_.ki = PID_I_MAX_; 54 | } 55 | if (!ros::param::get("Kd", pidGains_.kd)) { 56 | ROS_INFO("no 'Kd' set; using the default value: %f", PID_D_DEFAULT_); 57 | pidGains_.kd = PID_D_DEFAULT_; 58 | } 59 | if (pidGains_.kd < PID_D_MIN_) { 60 | ROS_WARN("pidGains_.Kd is too low, changing to: %f", PID_D_MIN_); 61 | pidGains_.kd = PID_D_MIN_; 62 | } else if (pidGains_.kd > PID_D_MAX_) { 63 | ROS_WARN("pidGains_.Kd is too high, changing to: %f", PID_D_MAX_); 64 | pidGains_.kd = PID_D_MAX_; 65 | } 66 | //~PID Control 67 | if (!ros::param::get("angular_coef", odom_angular_coef_)) { 68 | ROS_INFO("no 'angular_coef' set; using the default value: %f", 69 | ODOM_ANGULAR_COEF_DEFAULT_); 70 | odom_angular_coef_ = ODOM_ANGULAR_COEF_DEFAULT_; 71 | } 72 | if (!ros::param::get("traction_factor", odom_traction_factor_)) { 73 | ROS_INFO("no 'traction_factor' set; using the default value: %f", 74 | ODOM_TRCTION_FACTOR_DEFAULT_); 75 | odom_traction_factor_ = ODOM_TRCTION_FACTOR_DEFAULT_; 76 | } 77 | if (!ros::param::get("angular_a_coef", angular_scaling_params_.a_coef)) { 78 | ROS_INFO("no 'angular_a_coef' set; using the default value: %f", 79 | ANGULAR_SCALING_A_DEFAULT_); 80 | angular_scaling_params_.a_coef = ANGULAR_SCALING_A_DEFAULT_; 81 | } 82 | if (!ros::param::get("angular_b_coef", angular_scaling_params_.b_coef)) { 83 | ROS_INFO("no 'angular_b_coef' set; using the default value: %f", 84 | ANGULAR_SCALING_B_DEFAULT_); 85 | angular_scaling_params_.b_coef = ANGULAR_SCALING_B_DEFAULT_; 86 | } 87 | if (!ros::param::get("angular_c_coef", angular_scaling_params_.c_coef)) { 88 | ROS_INFO("no 'angular_c_coef' set; using the default value: %f", 89 | ANGULAR_SCALING_C_DEFAULT_); 90 | angular_scaling_params_.c_coef = ANGULAR_SCALING_C_DEFAULT_; 91 | } 92 | if (!ros::param::get("angular_min_scale", 93 | angular_scaling_params_.min_scale_val)) { 94 | ROS_INFO("no 'angular_min_scale' set; using the default value: %f", 95 | ANGULAR_SCALING_MIN_DEFAULT_); 96 | angular_scaling_params_.min_scale_val = ANGULAR_SCALING_MIN_DEFAULT_; 97 | } 98 | if (!ros::param::get("angular_max_scale", 99 | angular_scaling_params_.max_scale_val)) { 100 | ROS_INFO("no 'angular_max_scale' set; using the default value: %f", 101 | ANGULAR_SCALING_MAX_DEFAULT_); 102 | angular_scaling_params_.max_scale_val = ANGULAR_SCALING_MAX_DEFAULT_; 103 | } 104 | if (!ros::param::get("robot_type", robot_type_)) { 105 | ROS_FATAL( 106 | "No 'robot_type' found as a parameter. Shutting down Driver Node"); 107 | ros::shutdown(); 108 | } else if (robot_type_ == "pro") { 109 | robot_ = std::make_unique( 110 | device_port_.c_str(), comm_type_, robot_mode_, pidGains_); 111 | } else if (robot_type_ == "pro2") { 112 | robot_ = std::make_unique( 113 | device_port_.c_str(), comm_type_, robot_mode_, pidGains_, 114 | angular_scaling_params_); 115 | } else if (robot_type_ == "zero2") { 116 | robot_ = std::make_unique( 117 | device_port_.c_str(), comm_type_, robot_mode_, pidGains_, 118 | angular_scaling_params_); 119 | } else if (robot_type_ == "mini") { 120 | robot_ = std::make_unique( 121 | device_port_.c_str(), comm_type_, robot_mode_, pidGains_, 122 | angular_scaling_params_); 123 | } else if (robot_type_ == "miti") { 124 | robot_ = std::make_unique( 125 | device_port_.c_str(), comm_type_, robot_mode_, pidGains_, 126 | angular_scaling_params_); 127 | } else { 128 | ROS_FATAL("Unknown Robot Type. Shutting down ROS"); 129 | ros::shutdown(); 130 | return; 131 | } 132 | 133 | // Odom frame and Child frame params 134 | if (!ros::param::get("odom_frame", odom_frame_id_)) { 135 | ROS_INFO("no 'odom_frame_id_' set; using the default value: %f", 136 | ODOM_FRAME_ID_DEFAULT_); 137 | odom_frame_id_ = ODOM_FRAME_ID_DEFAULT_; 138 | } 139 | if (!ros::param::get("odom_child_frame", odom_child_frame_id_)) { 140 | ROS_INFO("no 'odom_child_frame_id_' set; using the default value: %f", 141 | ODOM_CHILD_FRAME_ID_DEFAULT_); 142 | odom_child_frame_id_ = ODOM_CHILD_FRAME_ID_DEFAULT_; 143 | } 144 | 145 | 146 | // Check if launch files have parameters set; Otherwise use hardcoded values 147 | if (!ros::param::get("trim_topic", trim_topic_)) { 148 | ROS_INFO("no 'trim_topic' set; using the default value: '/trim_increment'"); 149 | trim_topic_ = "/trim_increment"; 150 | } 151 | if (!ros::param::get("speed_topic", speed_topic_)) { 152 | ROS_INFO( 153 | "no 'speed_topic' set; using the default value: '/cmd_vel/managed'"); 154 | speed_topic_ = "/cmd_vel/managed"; 155 | } 156 | if (!ros::param::get("estop_trigger_topic", estop_trigger_topic_)) { 157 | ROS_INFO( 158 | "no 'estop_trigger_topic' set; using the default value: " 159 | "'/soft_estop/trigger'"); 160 | estop_trigger_topic_ = "/soft_estop/trigger"; 161 | } 162 | if (!ros::param::get("estop_reset_topic", estop_reset_topic_)) { 163 | ROS_INFO( 164 | "no 'estop_reset_topic' set; using the default value: " 165 | "'/soft_estop/reset'"); 166 | estop_reset_topic_ = "/soft_estop/reset"; 167 | } 168 | if (!ros::param::get("status_topic", robot_status_topic_)) { 169 | ROS_INFO("no 'status_topic' set; using the default value: '/robot_status'"); 170 | robot_status_topic_ = "/robot_status"; 171 | } 172 | if (!ros::param::get("status_frequency", robot_status_frequency_)) { 173 | ROS_INFO("no 'status_frequency' set; using the default value: %f", 174 | ROBOT_STATUS_FREQUENCY_DEFAULT_); 175 | robot_status_frequency_ = ROBOT_STATUS_FREQUENCY_DEFAULT_; 176 | } 177 | if (robot_status_frequency_ < ROBOT_STATUS_FREQUENCY_MIN_) { 178 | ROS_WARN("status_frequency is too low, changing to default value: %f", 179 | ROBOT_STATUS_FREQUENCY_MIN_); 180 | robot_status_frequency_ = ROBOT_STATUS_FREQUENCY_MIN_; 181 | } else if (robot_status_frequency_ > ROBOT_STATUS_FREQUENCY_MAX_) { 182 | ROS_WARN("status_frequency is too high, changing to default value: %f", 183 | ROBOT_STATUS_FREQUENCY_MAX_); 184 | robot_status_frequency_ = ROBOT_STATUS_FREQUENCY_MAX_; 185 | } 186 | if (!ros::param::get("odom_frequency", robot_odom_frequency_)) { 187 | robot_odom_frequency_ = ROBOT_ODOM_FREQUENCY_DEFAULT_; 188 | ROS_INFO("no 'odom_frequency' set; using the default value: %f", 189 | robot_odom_frequency_); 190 | } 191 | if (!ros::param::get("info_request_topic", robot_info_request_topic_)) { 192 | ROS_INFO( 193 | "no 'info_request_topic' set; using the default value: " 194 | "'/robot_request_info'"); 195 | robot_info_request_topic_ = "/robot_request_info"; 196 | } 197 | if (!ros::param::get("info_topic", robot_info_topic_)) { 198 | ROS_INFO( 199 | "no 'info_topic' set; using the default value: '/robot_unique_info'"); 200 | robot_info_topic_ = "/robot_unique_info"; 201 | } 202 | if (!ros::param::get("mode_trigger_topic", mode_trigger_topic_)) { 203 | ROS_INFO( 204 | "no 'mode_trigger_topic' set; using the default value: '/mode_toggle'"); 205 | mode_trigger_topic_ = "/mode_toggle"; 206 | } 207 | 208 | trim_command_subscriber_ = 209 | nh->subscribe(trim_topic_, 1, &RobotDriver::callbackTrim, this); 210 | speed_command_subscriber_ = 211 | nh->subscribe(speed_topic_, 10, &RobotDriver::callbackSpeedCommand, this); 212 | mode_trigger_subscriber_ = nh->subscribe( 213 | mode_trigger_topic_, 10, &RobotDriver::callbackModeTrigger, this); 214 | estop_trigger_subscriber_ = nh->subscribe( 215 | estop_trigger_topic_, 10, &RobotDriver::callbackEstopTrigger, this); 216 | estop_reset_subscriber_ = nh->subscribe( 217 | estop_reset_topic_, 10, &RobotDriver::callbackEstopReset, this); 218 | robot_info_subscriber_ = nh->subscribe(robot_info_request_topic_, 10, 219 | &RobotDriver::callbackInfo, this); 220 | robot_info_publisher_ = nh->advertise( 221 | robot_info_topic_, 1); // publish robot_unique info 222 | robot_status_publisher_ = 223 | nh->advertise(robot_status_topic_, 10); 224 | robot_status_timer_ = 225 | nh->createTimer(ros::Duration(1.0 / robot_status_frequency_), 226 | &RobotDriver::publishRobotStatus, this); 227 | robot_odom_publisher_ = nh->advertise("odom", 1); 228 | // feedback_publisher_ = 229 | // nh->advertise("/set_feedback", 1); 230 | odom_publish_timer_ = 231 | nh->createTimer(ros::Duration(1.0 / robot_odom_frequency_), 232 | &RobotDriver::publishOdometry, this); 233 | ROS_INFO("Subscribers and Publishers are running..."); 234 | ROS_INFO("ROBOT ESTOP STATE %d", estop_state_); 235 | } 236 | 237 | void RobotDriver::publishRobotStatus(const ros::TimerEvent &event) { 238 | if (!robot_->is_connected()) { 239 | ROS_FATAL( 240 | "Unexpectedly disconnected from serial port. Check connection to " 241 | "robot " 242 | "and reset Estop"); 243 | robot_status_timer_.stop(); 244 | ros::shutdown(); 245 | } 246 | robotData data = robot_->status_request(); 247 | std_msgs::Float32MultiArray robot_status; 248 | robot_status.data.clear(); 249 | // Motor Infos 250 | robot_status.data.push_back(data.motor1_id); 251 | robot_status.data.push_back(data.motor1_rpm); 252 | robot_status.data.push_back(data.motor1_current); 253 | robot_status.data.push_back(data.motor1_temp); 254 | robot_status.data.push_back(data.motor1_mos_temp); 255 | robot_status.data.push_back(data.motor2_id); 256 | robot_status.data.push_back(data.motor2_rpm); 257 | robot_status.data.push_back(data.motor2_current); 258 | robot_status.data.push_back(data.motor2_temp); 259 | robot_status.data.push_back(data.motor2_mos_temp); 260 | robot_status.data.push_back(data.motor3_id); 261 | robot_status.data.push_back(data.motor3_rpm); 262 | robot_status.data.push_back(data.motor3_current); 263 | robot_status.data.push_back(data.motor3_temp); 264 | robot_status.data.push_back(data.motor3_mos_temp); 265 | robot_status.data.push_back(data.motor4_id); 266 | robot_status.data.push_back(data.motor4_rpm); 267 | robot_status.data.push_back(data.motor4_current); 268 | robot_status.data.push_back(data.motor4_temp); 269 | robot_status.data.push_back(data.motor4_mos_temp); 270 | // Battery Infos 271 | robot_status.data.push_back(data.battery1_voltage); 272 | robot_status.data.push_back(data.battery2_voltage); 273 | robot_status.data.push_back(data.battery1_temp); 274 | robot_status.data.push_back(data.battery2_temp); 275 | robot_status.data.push_back(data.battery1_current); 276 | robot_status.data.push_back(data.battery2_current); 277 | robot_status.data.push_back(data.battery1_SOC); 278 | robot_status.data.push_back(data.battery2_SOC); 279 | robot_status.data.push_back(data.battery1_fault_flag); 280 | robot_status.data.push_back(data.battery2_fault_flag); 281 | 282 | // Flipper Infos 283 | robot_status.data.push_back(data.motor3_angle); 284 | robot_status.data.push_back(data.motor3_sensor1); 285 | robot_status.data.push_back(data.motor3_sensor2); 286 | 287 | robot_status_publisher_.publish(robot_status); 288 | // ROS_INFO("publishing some robot state"); 289 | } 290 | 291 | void RobotDriver::publishOdometry(const ros::TimerEvent &event) { 292 | robotData data = robot_->status_request(); 293 | nav_msgs::Odometry odom_msg; 294 | static double pos_x = 0; 295 | static double pos_y = 0; 296 | static double theta = 0; 297 | static double past_time = 0; 298 | double now_time = 0; 299 | double dt = 0; 300 | float odom_covariance_0_ = 0.01; 301 | float odom_covariance_35_ = 0.03; 302 | tf2::Quaternion q_new; 303 | 304 | odom_msg.header.stamp = ros::Time::now(); 305 | odom_msg.header.frame_id = odom_frame_id_; 306 | odom_msg.child_frame_id = odom_child_frame_id_; 307 | 308 | // Calculate time 309 | ros::Time ros_now_time = ros::Time::now(); 310 | now_time = ros_now_time.toSec(); 311 | 312 | dt = now_time - past_time; 313 | past_time = now_time; 314 | 315 | // Calculate position 316 | if (past_time != 0) 317 | { 318 | pos_x = pos_x + data.linear_vel * cos(theta) * dt; 319 | pos_y = pos_y + data.linear_vel * sin(theta) * dt; 320 | theta = (theta + data.angular_vel * dt); 321 | 322 | q_new.setRPY(0, 0, theta); 323 | tf2::convert(q_new, odom_msg.pose.pose.orientation); 324 | } 325 | 326 | odom_msg.pose.pose.position.x = pos_x; 327 | odom_msg.pose.pose.position.y = pos_y; 328 | 329 | odom_msg.twist.twist.linear.x = data.linear_vel; 330 | odom_msg.twist.twist.angular.z = data.angular_vel; 331 | 332 | // Covariance: 333 | // If not moving, trust the encoders completely 334 | // Otherwise set them to the ROS param 335 | if (data.linear_vel == 0 && data.angular_vel == 0) 336 | { 337 | odom_msg.twist.covariance[0] = odom_covariance_0_ / 1e3; 338 | odom_msg.twist.covariance[7] = odom_covariance_0_ / 1e3; 339 | odom_msg.twist.covariance[35] = odom_covariance_35_ / 1e6; 340 | } 341 | else 342 | { 343 | odom_msg.twist.covariance[0] = odom_covariance_0_; 344 | odom_msg.twist.covariance[7] = odom_covariance_0_; 345 | odom_msg.twist.covariance[35] = odom_covariance_35_; 346 | } 347 | 348 | robot_odom_publisher_.publish(odom_msg); 349 | } 350 | 351 | void RobotDriver::publishRobotInfo() { 352 | if (!robot_->is_connected()) { 353 | ROS_FATAL( 354 | "Unexpectedly disconnected from serial port. Check connection to " 355 | "robot " 356 | "and reset Estop"); 357 | robot_status_timer_.stop(); 358 | ros::shutdown(); 359 | return; 360 | } 361 | robotData data = robot_->info_request(); 362 | std_msgs::Float32MultiArray robot_info; 363 | robot_info.data.clear(); 364 | robot_info.data.push_back(data.robot_guid); 365 | robot_info.data.push_back(data.robot_firmware); 366 | robot_info.data.push_back(data.robot_speed_limit); 367 | robot_info.data.push_back(data.robot_fan_speed); 368 | robot_info.data.push_back(data.robot_fault_flag); 369 | robot_info_publisher_.publish(robot_info); 370 | } 371 | // call everytime speed_topic_ get data 372 | void RobotDriver::callbackSpeedCommand(const geometry_msgs::Twist &msg) { 373 | double velocity_data[3]; 374 | velocity_data[0] = msg.linear.x; 375 | velocity_data[1] = msg.angular.z; 376 | velocity_data[2] = msg.angular.y; 377 | robot_->set_robot_velocity(velocity_data); 378 | } 379 | 380 | void RobotDriver::callbackModeTrigger(const std_msgs::Bool::ConstPtr &msg) { 381 | int mode; 382 | sensor_msgs::JoyFeedbackArray a; 383 | if (msg->data) { 384 | mode = robot_->cycle_robot_mode(); 385 | ROS_INFO("new mode %d", mode); 386 | switch (mode) { 387 | case Control::OPEN_LOOP: 388 | ROS_INFO("Robot Mode : Open Loop"); 389 | break; 390 | case Control::TRACTION_CONTROL: 391 | ROS_INFO("Robot Mode : TRACTION_CONTROL"); 392 | break; 393 | case Control::INDEPENDENT_WHEEL: 394 | ROS_INFO("Robot Mode : INDEPENDENT_WHEEL"); 395 | 396 | break; 397 | } 398 | } 399 | 400 | } 401 | 402 | void RobotDriver::callbackInfo(const std_msgs::Bool::ConstPtr &msg) { 403 | if (msg->data) { 404 | publishRobotInfo(); 405 | } 406 | } 407 | 408 | void RobotDriver::callbackEstopTrigger(const std_msgs::Bool::ConstPtr &msg) { 409 | if (msg->data == true) { 410 | estop_state_ = true; 411 | robot_->send_estop(estop_state_); 412 | } 413 | } 414 | 415 | void RobotDriver::callbackEstopReset(const std_msgs::Bool::ConstPtr &msg) { 416 | if (msg->data == true) { 417 | estop_state_ = false; 418 | robot_->send_estop(estop_state_); 419 | } 420 | } 421 | 422 | void RobotDriver::callbackTrim(const std_msgs::Float32::ConstPtr &msg) { 423 | if (msg->data != 0) { 424 | robot_->update_drivetrim(msg->data); 425 | } 426 | } 427 | 428 | RobotDriver::~RobotDriver() {} 429 | } // namespace RoverRobotics 430 | int main(int argc, char **argv) { 431 | ros::init(argc, argv, "RoverRobotics_Driver_Wrapper_Node"); 432 | ros::NodeHandle nh; 433 | ros::AsyncSpinner spinner(0); // Prevent Callback bottleneck 434 | spinner.start(); 435 | RoverRobotics::RobotDriver robot(&nh); 436 | ROS_INFO("Robot driver is started"); 437 | 438 | ros::waitForShutdown(); 439 | // robot.~RobotDriver(); 440 | return 0; 441 | } 442 | -------------------------------------------------------------------------------- /roverrobotics_input_manager/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package roverrobotics_input_manager 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 2.0.1 (2020-12-14) 5 | ------------------ 6 | 7 | 2.0.0 (2020-11-10) 8 | ------------------ 9 | * add support for ps4 controller, remove support for xbox controller. 10 | * add trim control 11 | * add estop control 12 | 13 | 1.2.0 (2020-11-05) 14 | ------------------ 15 | 16 | 1.1.1 (2020-06-15) 17 | ------------------ 18 | 19 | 1.1.0 (2020-06-08) 20 | ------------------ 21 | * release to melodic 22 | 23 | 1.0.0 (2020-06-08) 24 | ------------------ 25 | * adding support for freedom robotics and add watchdog timer to rover zero driver 26 | * Contributors: padiln 27 | 28 | 0.8.0 (2020-03-25) 29 | ------------------ 30 | 31 | 0.7.4 (2020-02-19) 32 | ------------------ 33 | * changed maintainership 34 | 35 | 0.7.3 (2019-10-14) 36 | ------------------ 37 | 38 | 0.7.2 (2019-07-18) 39 | ------------------ 40 | * Changed roverrobotics_input_manager from a mux with fix input to latency manager for TwistStamped messages. 41 | 42 | 0.7.1 (2019-06-24) 43 | ------------------ 44 | 45 | 0.7.0 (2019-05-13) 46 | ------------------ 47 | 48 | 0.6.2 (2019-01-23) 49 | ------------------ 50 | 51 | 0.6.1 (2018-12-20) 52 | ------------------ 53 | 54 | 0.6.0 (2018-12-07) 55 | ------------------ 56 | 57 | 0.5.1 (2018-09-20) 58 | ------------------ 59 | 60 | 0.5.0 (2018-09-06) 61 | ------------------ 62 | 63 | 0.4.0 (2018-07-23) 64 | ------------------ 65 | 66 | 0.3.0 (2018-07-19) 67 | ------------------ 68 | 69 | 0.2.0 (2018-07-17) 70 | ------------------ 71 | 72 | 0.1.0 (2018-07-10) 73 | ------------------ 74 | -------------------------------------------------------------------------------- /roverrobotics_input_manager/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(roverrobotics_input_manager) 3 | 4 | 5 | find_package(catkin REQUIRED COMPONENTS 6 | roscpp 7 | rospy 8 | std_msgs 9 | ) 10 | 11 | #add_message_files( FILES 12 | # Status.msg) 13 | 14 | ################################################ 15 | ## Declare ROS messages, services and actions ## 16 | ################################################ 17 | 18 | 19 | ################################################ 20 | ## Declare ROS dynamic reconfigure parameters ## 21 | ################################################ 22 | 23 | 24 | ################################### 25 | ## catkin specific configuration ## 26 | ################################### 27 | catkin_package( 28 | # INCLUDE_DIRS include 29 | # LIBRARIES roverrobotics_input_manager 30 | # CATKIN_DEPENDS roscpp rospy std_msgs 31 | # DEPENDS system_lib 32 | ) 33 | 34 | ########### 35 | ## Build ## 36 | ########### 37 | include_directories( 38 | # include 39 | ${catkin_INCLUDE_DIRS} 40 | ) 41 | 42 | 43 | ############# 44 | ## Install ## 45 | ############# 46 | 47 | catkin_install_python(PROGRAMS scripts/control_input_manager.py scripts/ps4_mapper.py 48 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 49 | ) 50 | 51 | catkin_install_python(PROGRAMS test/test_functional.py test/test_latency.py 52 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/test 53 | ) 54 | 55 | ## Copy launch files 56 | install(DIRECTORY launch/ 57 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 58 | FILES_MATCHING PATTERN "*.launch" 59 | ) 60 | 61 | ## Copy config files 62 | install(DIRECTORY config/ 63 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config 64 | FILES_MATCHING PATTERN "*.yaml" 65 | ) 66 | 67 | ## Copy test files 68 | install(DIRECTORY test/ 69 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/test 70 | FILES_MATCHING PATTERN "*.test" 71 | ) 72 | 73 | ############# 74 | ## Testing ## 75 | ############# 76 | 77 | ## Add gtest based cpp test target and link libraries 78 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_roverrobotics_input_manager.cpp) 79 | # if(TARGET ${PROJECT_NAME}-test) 80 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 81 | # endif() 82 | 83 | ## Add folders to be run by python nosetests 84 | # catkin_add_nosetests(test) 85 | 86 | if(CATKIN_ENABLE_TESTING) 87 | find_package(rostest REQUIRED) 88 | add_rostest(test/control_input_manager.test) 89 | endif() 90 | -------------------------------------------------------------------------------- /roverrobotics_input_manager/LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2018, Rover Robotics 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /roverrobotics_input_manager/config/controller_config.yaml: -------------------------------------------------------------------------------- 1 | stamped: false 2 | inputs: 3 | linear: 4 | x: axis_left_y 5 | angular: 6 | # Use L2 and R2 for rotation 7 | # z: axis_l2 - axis_r2 8 | # Use right horizontal stick for rotation 9 | y: -axis_right_y 10 | z: axis_right_x 11 | scales: 12 | linear: 13 | x: 1 14 | angular: 15 | y: 50 16 | z: 3.14 17 | -------------------------------------------------------------------------------- /roverrobotics_input_manager/config/input_topics.yaml: -------------------------------------------------------------------------------- 1 | # Input stamped command velocities 2 | # For each topic: 3 | # - sub_topic : name of topic to subscribe to that publishes geometry_msgs::TwistStamped 4 | # - pub_topic : name of topic to publish the managed output 5 | # - timeout : timeout in seconds to start discarding old messages with expired stamps. 0.0 or less never trigger timeout 6 | # - stamped : Boolean that specifies whether the output from the relay should be TwistStamped of Twist 7 | # 8 | # The cmd_vel/freedom topic is specifically dedicated for the use by https://www.freedomrobotics.ai. 9 | # The timeout is set to 0.0 because network latency can vary greatly and should be set to meet your needs. 10 | 11 | control_inputs: 12 | - 13 | sub_topic: cmd_vel/joystick 14 | pub_topic: /managed/joy 15 | timeout : 0.5 16 | stamped : false 17 | publish_redundant_halts: false 18 | - 19 | sub_topic: keyboard 20 | pub_topic: /managed/key 21 | timeout : 0.5 22 | stamped : false 23 | - 24 | sub_topic: cmd_vel/auto_dock 25 | pub_topic: /managed/auto_dock 26 | timeout : 0.0 27 | stamped : false 28 | - 29 | sub_topic: cmd_vel/freedom 30 | pub_topic: /managed/freedom 31 | timeout : 0.0 32 | stamped : false 33 | -------------------------------------------------------------------------------- /roverrobotics_input_manager/config/test.yaml: -------------------------------------------------------------------------------- 1 | # Input stamped command velocities 2 | # For each topic: 3 | # - sub_topic: name of topic to subscribe to that publishes geometry_msgs::TwistStamped 4 | # - pub_topic: name of topic to publish the managed output 5 | # - timeout : timeout in seconds to start discarding old messages with expired stamps. 0.0 or less never trigger timeout 6 | # - stamped : Boolean that specifies whether the output from the relay should be TwistStamped of Twist 7 | 8 | control_inputs: 9 | - 10 | sub_topic: /test/inputA 11 | pub_topic: /test/outputA 12 | timeout : 0.0 13 | stamped : false 14 | - 15 | sub_topic: /test/inputB 16 | pub_topic: /test/outputB 17 | timeout : 1.0 18 | stamped : false 19 | - 20 | sub_topic: /test/inputC 21 | pub_topic: /test/outputC 22 | timeout : -1.0 23 | stamped : false 24 | - 25 | sub_topic: /test/inputD 26 | pub_topic: /test/outputD 27 | timeout : -notNum 28 | stamped : false 29 | - 30 | sub_topic: /test/inputE 31 | pub_topic: /test/outputE 32 | timeout : 0.0 33 | stamped : notBool 34 | - 35 | sub_topic: /test/inputF 36 | pub_topic: /test/outputF 37 | timeout : 0.0 38 | stamped : true 39 | - 40 | sub_topic: /test/inputG 41 | pub_topic: /test/outputG 42 | timeout : 0.0 43 | stamped : false 44 | - 45 | sub_topic: /test/inputH 46 | pub_topic: /test/outputH 47 | timeout : 1.0 48 | stamped : false -------------------------------------------------------------------------------- /roverrobotics_input_manager/launch/example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /roverrobotics_input_manager/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | roverrobotics_input_manager 4 | 2.0.1 5 | 6 | Filter velocity commands by ensuring that message time stamps do not exceed given timeout thresholds. 7 | 8 | 9 | 10 | 11 | 12 | John Bartholomew 13 | 14 | BSD 15 | 16 | catkin 17 | roscpp 18 | rospy 19 | std_msgs 20 | roscpp 21 | rospy 22 | std_msgs 23 | roscpp 24 | rospy 25 | std_msgs 26 | ds4_driver 27 | rostest 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /roverrobotics_input_manager/scripts/control_input_manager.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | from geometry_msgs.msg import TwistStamped, Twist 4 | 5 | HALT = Twist() 6 | 7 | class CommandHandle(object): 8 | """Manages timeouts and relaying incoming messages for a given control input""" 9 | # Class variables 10 | prev_cmd = None 11 | 12 | def __init__(self, sub_topic, pub_topic, timeout, stamped, sub_is_stamped=True, publish_redundant_halts=True, frame_id=''): 13 | self.sub_topic = sub_topic 14 | self.pub_topic = pub_topic 15 | self.timeout = float(timeout) 16 | self.is_stamped = stamped 17 | self.sub_is_stamped = sub_is_stamped 18 | self.publish_redundant_halts = publish_redundant_halts 19 | self.frame_id = frame_id 20 | self._pub_type = TwistStamped if stamped else Twist 21 | self._sub_type = TwistStamped if sub_is_stamped else Twist 22 | 23 | try: 24 | self.timeout = float(timeout) 25 | except ValueError: 26 | raise ValueError('Invalid `timeout` value for control_input: {TIMEOUT}' 27 | .format(TIMEOUT=timeout)) 28 | 29 | if not isinstance(stamped, bool): 30 | raise ValueError('Invalid `stamped` value for control_input: {STAMPED}' 31 | .format(STAMPED=stamped)) 32 | 33 | self.pub = rospy.Publisher(self.pub_topic, self._pub_type, queue_size=1) 34 | self.sub = rospy.Subscriber(self.sub_topic, self._sub_type, self.callback, queue_size=5) 35 | 36 | rospy.loginfo( 37 | 'Configuring Command Input Handler\n' + 38 | 'Subscribed Topic: {TOPIC}\n'.format(TOPIC=self.sub_topic) + 39 | 'Published Topic: {TOPIC}\n'.format(TOPIC=self.pub_topic) + 40 | 'Timeout: {TIMEOUT}\n'.format(TIMEOUT=self.timeout) + 41 | 'Input Topic Stamped: {STAMPED}\n'.format(STAMPED=self.sub_is_stamped) + 42 | 'Output Topic Stamped: {STAMPED}\n'.format(STAMPED=self.is_stamped) + 43 | 'Publishing Redundant Halt Commands: {HALT}\n'.format(HALT=self.publish_redundant_halts) 44 | ) 45 | 46 | def callback(self, data): 47 | """Passes through message and strip strips header if necessary""" 48 | if self.sub_is_stamped: 49 | twist_cmd = data.twist 50 | else: 51 | twist_cmd = data 52 | 53 | if not self.publish_redundant_halts and (self.prev_cmd == HALT and twist_cmd == HALT): 54 | return 55 | 56 | self.prev_cmd = twist_cmd 57 | 58 | if type(data) == TwistStamped and self.expired(data): 59 | return 60 | 61 | self.pub.publish(self.format_cmd_data(data)) 62 | 63 | 64 | def expired(self, data): 65 | """Tests message stamp to see if it has timeout""" 66 | now = rospy.Time.now() 67 | return 0.0 < self.timeout <= (now - data.header.stamp).to_sec() 68 | 69 | def format_cmd_data(self, data): 70 | if (self._sub_type == TwistStamped and self._pub_type == TwistStamped) or \ 71 | (self._sub_type == Twist and self._pub_type == Twist): 72 | return data 73 | elif self._sub_type == TwistStamped and self._pub_type == Twist: 74 | return data.twist 75 | else: 76 | data_stamped = TwistStamped() 77 | data_stamped.header.frame_id = self.frame_id 78 | data_stamped.header.stamp = rospy.Time.now() 79 | data_stamped.twist = data 80 | return data_stamped 81 | 82 | 83 | class ControlInputManager: 84 | """Creates a handle for each input. Drop inputs if timeout is not a number or 85 | stamped is not a boolean. 86 | """ 87 | 88 | def __init__(self, control_inputs): 89 | self.input_handles = [] 90 | for idx, control_input in enumerate(control_inputs): 91 | try: 92 | command_handle = CommandHandle(**control_input) 93 | self.input_handles.append(command_handle) 94 | except ValueError as e: 95 | rospy.logfatal('control_input manager ({IDX}) was not created. {params}' 96 | .format(IDX=idx, params=control_input)) 97 | rospy.logfatal(e) 98 | 99 | def run(self): 100 | rospy.spin() 101 | 102 | 103 | def check_params(control_inputs): 104 | required_parameters = {'pub_topic', 'sub_topic', 'timeout', 'stamped'} 105 | optional_parameters = {'sub_is_stamped', 'publish_redundant_halts'} 106 | for control_input in control_inputs: 107 | params = set(control_input.keys()) 108 | missing_params = required_parameters - params 109 | unrecognized_params = params - required_parameters - optional_parameters 110 | if len(missing_params) != 0: 111 | err_msg = "Missing the parameters: " + ', '.join(list(missing_params)) 112 | raise ValueError(err_msg) 113 | elif len(unrecognized_params) != 0: 114 | rospy.logerr('Unrecognized parameters: ' + ', '.join(list(unrecognized_params))) 115 | 116 | 117 | def main(): 118 | rospy.init_node('control_input_manager') 119 | control_inputs = rospy.get_param('~control_inputs') 120 | check_params(control_inputs) 121 | cim = ControlInputManager(control_inputs) 122 | cim.run() 123 | 124 | 125 | if __name__ == '__main__': 126 | main() 127 | -------------------------------------------------------------------------------- /roverrobotics_input_manager/scripts/ps4_mapper.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Description: This script converts Ds4 Status into Joint Velocity Commands with addtional button publishers 4 | # Ps4 Controller mapping: 5 | # Axes : axis_left_y axis_right_x axis_right_y axis_l2 axis_r2 6 | # Buttons: button_dpad_up button_dpad_down button_dpad_left button_dpad_right button_cross 7 | # button_circle button_square button_triangle 8 | # button_l1 button_l2button_l3 button_r1 button_r2 button_r3 button_share button_options button_trackpad button_ps 9 | 10 | import rospy 11 | from geometry_msgs.msg import Twist, TwistStamped 12 | from std_msgs.msg import Bool, Float32 13 | from ds4_driver.msg import Status, Feedback 14 | 15 | TRIM_DELTA = 0.01 16 | BIG_DEBOUNCE = 200 17 | SMALL_DEBOUNCE = 50 18 | class ps4_mapper(object): 19 | def __init__(self): 20 | self._stamped = rospy.get_param('~stamped', False) 21 | if self._stamped: 22 | self._cls = TwistStamped 23 | self._frame_id = rospy.get_param('~frame_id', 'base_link') 24 | else: 25 | self._cls = Twist 26 | self._inputs = rospy.get_param('~inputs') 27 | self._scales = rospy.get_param('~scales') 28 | self.buttonpressed = False 29 | self.togglebuttonpressed = False 30 | self.counter2 = 0 31 | self.counter = 0 32 | self._attrs = [] 33 | for attr in Status.__slots__: 34 | if attr.startswith('axis_') or attr.startswith('button_'): 35 | self._attrs.append(attr) 36 | self._feedback = Feedback() 37 | self._pub = rospy.Publisher( 38 | 'cmd_vel/joystick', self._cls, queue_size=1) 39 | self._pub_squ = rospy.Publisher('/mode_toggle', Bool, queue_size=1) 40 | self._pub_triangle = rospy.Publisher( 41 | '/joystick/triangle', Bool, queue_size=1, latch=True) 42 | self._pub_circle = rospy.Publisher( 43 | '/soft_estop/reset', Bool, queue_size=1) # , latch =True) 44 | self._pub_cross = rospy.Publisher( 45 | '/soft_estop/trigger', Bool, queue_size=1) # , latch =True) 46 | self._pub_trim = rospy.Publisher( 47 | '/trim_increment', Float32, queue_size=1) 48 | self._trim_incre_value = rospy.get_param('~trim_increment_value', 0.01) 49 | self._pub_feedback = rospy.Publisher( 50 | "set_feedback", Feedback, queue_size=1) 51 | rospy.Subscriber('status', Status, self.cb_status, queue_size=1) 52 | rospy.loginfo("Linear Scale is at %f", self._scales["linear"]["x"]) 53 | self.default_linear = self._scales["linear"]["x"] 54 | self.default_angular = self._scales["angular"]["z"] 55 | self._feedback.set_led = True 56 | self._feedback.led_g = 255 57 | self._pub_feedback.publish(self._feedback) 58 | 59 | def cb_status(self, msg): 60 | """ 61 | :param msg: 62 | :type msg: Status 63 | :return: 64 | """ 65 | 66 | input_vals = {} 67 | for attr in self._attrs: 68 | input_vals[attr] = getattr(msg, attr) 69 | 70 | to_pub = self._cls() 71 | if self._stamped: 72 | to_pub.header.stamp = rospy.Time.now() 73 | to_pub.header.frame_id = self._frame_id 74 | twist = to_pub.twist 75 | else: 76 | twist = to_pub 77 | # go through each velocity input types 78 | for vel_type in self._inputs: 79 | vel_vec = getattr(twist, vel_type) 80 | for k, expr in self._inputs[vel_type].items(): 81 | scale = self._scales[vel_type].get(k, 1.0) 82 | val = eval(expr, {}, input_vals) 83 | setattr(vel_vec, k, scale * val) 84 | if (msg.button_l1 or msg.button_r1) and self.buttonpressed is False: 85 | trim_msg = Float32() 86 | if msg.button_r1: 87 | trim_msg = TRIM_DELTA 88 | elif msg.button_l1: 89 | trim_msg = -TRIM_DELTA 90 | self._pub_trim.publish(trim_msg) 91 | self.buttonpressed = True 92 | elif self.buttonpressed: # Debounce 93 | self.counter += 1 94 | if self.counter == SMALL_DEBOUNCE: 95 | self.counter = 0 96 | self.buttonpressed = False 97 | self._feedback.set_rumble = False 98 | self._pub_feedback.publish(self._feedback) 99 | trim_msg = 0 100 | self._pub_trim.publish(trim_msg) 101 | 102 | if self.togglebuttonpressed: # Debounce mode 103 | self.counter2 += 1 104 | if self.counter2 == BIG_DEBOUNCE: 105 | self.counter2 = 0 106 | self.togglebuttonpressed = False 107 | if (msg.button_dpad_up or msg.button_dpad_down) and self.buttonpressed is False: 108 | if msg.button_dpad_up and self._scales["linear"].get("x") < 3: 109 | self._scales["linear"]["x"] += 0.05 110 | elif msg.button_dpad_down and self._scales["linear"].get("x") > 0.05: 111 | self._scales["linear"]["x"] -= 0.05 112 | elif self._scales["linear"].get("x") <= 0.06 or self._scales["linear"].get("x") >= 3: 113 | self._feedback.set_rumble = True 114 | rospy.loginfo("Limit Reach %f", 115 | self._scales["linear"].get("x")) 116 | self._feedback.rumble_big = 1 117 | self._pub_feedback.publish(self._feedback) 118 | rospy.loginfo('Linear Scale is at %f', self._scales["linear"]["x"]) 119 | self.buttonpressed = True 120 | if (msg.button_dpad_left or msg.button_dpad_right) and self.buttonpressed is False: 121 | if msg.button_dpad_right and self._scales["angular"].get("z") < 4.71: 122 | self._scales["angular"]["z"] += 0.05 123 | elif msg.button_dpad_left and self._scales["angular"].get("z") > 0.05: 124 | self._scales["angular"]["z"] -= 0.05 125 | elif self._scales["angular"].get("z") <= 0.06 or self._scales["angular"].get("z") >= 4.71: 126 | self._feedback.set_rumble = True 127 | rospy.loginfo("Limit Reach %f", 128 | self._scales["angular"].get("z")) 129 | self._feedback.rumble_big = 1 130 | self._pub_feedback.publish(self._feedback) 131 | rospy.loginfo('Angular Scale is at %f', self._scales["angular"]["z"]) 132 | self.buttonpressed = True 133 | if msg.button_l3 and self.buttonpressed is False: 134 | self._scales["linear"]["x"] = self.default_linear 135 | self._feedback.set_rumble = True 136 | self._feedback.rumble_big = 1 137 | self._pub_feedback.publish(self._feedback) 138 | self.buttonpressed = True 139 | 140 | if msg.button_r3 and self.buttonpressed is False: 141 | self._scales["angular"]["z"] = self.default_angular 142 | self._feedback.set_rumble = True 143 | self._feedback.rumble_big = 1 144 | self._pub_feedback.publish(self._feedback) 145 | self.buttonpressed = True 146 | 147 | 148 | button_msg = Bool() 149 | button_msg.data = False 150 | if msg.button_cross: 151 | button_msg = Bool() 152 | button_msg.data = True 153 | self._pub_cross.publish(button_msg) 154 | button_msg.data = False 155 | if msg.button_circle: 156 | button_msg = Bool() 157 | button_msg.data = True 158 | self._pub_circle.publish(button_msg) 159 | button_msg.data = False 160 | if msg.button_square and self.togglebuttonpressed is False: 161 | button_msg = Bool() 162 | button_msg.data = True 163 | self._pub_squ.publish(button_msg) 164 | button_msg.data = False 165 | self.togglebuttonpressed = True 166 | self._pub_circle.publish(button_msg) 167 | self._pub_cross.publish(button_msg) 168 | self._pub.publish(to_pub) 169 | 170 | 171 | def main(): 172 | 173 | rospy.init_node('ps4_mapper') 174 | ps4_mapper() 175 | rospy.spin() 176 | 177 | 178 | if __name__ == '__main__': 179 | main() 180 | -------------------------------------------------------------------------------- /roverrobotics_input_manager/test/control_input_manager.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /roverrobotics_input_manager/test/test_functional.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | import unittest 4 | import rosunit 5 | from geometry_msgs.msg import Twist, TwistStamped 6 | 7 | PKG = 'roverrobotics_input_manager' 8 | 9 | 10 | class TestTwistCommand(unittest.TestCase): 11 | def __init__(self, *args, **kwargs): 12 | super(TestTwistCommand, self).__init__(*args, **kwargs) 13 | self.message_received = False 14 | self.message_stamp = None 15 | self.msg = None 16 | self.pub = None 17 | self.sub = None 18 | 19 | def setUp(self): 20 | test_method_name = self._testMethodName 21 | subscribers = {'test_manager_unstamped': ('/test/outputA', Twist), 22 | 'test_manager_stamped': ('/test/outputF', TwistStamped), 23 | 'test_latency_management_zero': ('/test/outputG', Twist), 24 | 'test_latency_management_one': ('/test/outputH', Twist), 25 | 'test_latency_management_less_than_zero': ('/test/outputC', Twist) 26 | } 27 | publishers = {'test_manager_unstamped': ('/test/inputA', TwistStamped), 28 | 'test_manager_stamped': ('/test/inputF', TwistStamped), 29 | 'test_latency_management_zero': ('/test/inputG', TwistStamped), 30 | 'test_latency_management_one': ('/test/inputH', TwistStamped), 31 | 'test_latency_management_less_than_zero': ('/test/inputC', TwistStamped) 32 | } 33 | 34 | if test_method_name in subscribers.keys(): 35 | self.sub = rospy.Subscriber(subscribers[test_method_name][0], 36 | subscribers[test_method_name][1], 37 | self.listener, 38 | queue_size=10) 39 | if test_method_name in publishers.keys(): 40 | self.pub = rospy.Publisher(publishers[test_method_name][0], 41 | publishers[test_method_name][1], 42 | queue_size=1) 43 | 44 | # Allow time for the publisher and subscriber to properly connect to endpoints 45 | rospy.sleep(1) 46 | 47 | def tearDown(self): 48 | if self.sub: 49 | self.sub.unregister() 50 | if self.pub: 51 | self.pub.unregister() 52 | 53 | # rospy.signal_shutdown('Test {test_name} completed'.format(test_name=self._testMethodName)) 54 | 55 | def listener(self, data): 56 | self.message_stamp = rospy.Time.now() 57 | self.message_received = True 58 | self.msg = data 59 | 60 | def generate_twist(self, x=0, y=0, z=0, rx=0, ry=0, rz=0): 61 | msg = Twist() 62 | msg.linear.x = x 63 | msg.linear.y = y 64 | msg.linear.z = z 65 | msg.angular.x = rx 66 | msg.angular.y = ry 67 | msg.angular.z = rz 68 | 69 | return msg 70 | 71 | def generate_twist_stamped(self, time, seq=0, frame_id='', x=0, y=0, z=0, ax=0, ay=0, az=0): 72 | msg = TwistStamped() 73 | msg.header.seq = seq 74 | msg.header.stamp = time 75 | msg.header.frame_id = frame_id 76 | msg.twist.linear.x = x 77 | msg.twist.linear.y = y 78 | msg.twist.linear.z = z 79 | msg.twist.angular.x = ax 80 | msg.twist.angular.y = ay 81 | msg.twist.angular.z = az 82 | 83 | return msg 84 | 85 | def twist_stamped_eq(self, msg1, msg2): 86 | """Tests TwistStamped equality, disregarding header.seq. `seq` can be overwritten 87 | when published and is a deprecated feature, so it left out of the equality check..""" 88 | result = msg1.header.stamp == msg2.header.stamp \ 89 | and msg1.header.frame_id == msg2.header.frame_id \ 90 | and msg1.twist == msg2.twist 91 | 92 | return result 93 | 94 | def test_creates_proper_topics(self): # only functions with 'test_'-prefix will be run! 95 | topics = [item[0] for item in rospy.get_published_topics()] 96 | self.assertIn('/test/outputA', topics) 97 | self.assertIn('/test/outputB', topics) 98 | self.assertIn('/test/outputC', topics) 99 | self.assertIn('/test/outputF', topics) 100 | self.assertIn('/test/outputG', topics) 101 | 102 | def test_bad_timeout(self): # only functions with 'test_'-prefix will be run! 103 | topics = [item[0] for item in rospy.get_published_topics()] 104 | self.assertNotIn('/test/inputD', topics) 105 | self.assertNotIn('/test/outputD', topics) 106 | 107 | def test_bad_stamped(self): 108 | topics = [item[0] for item in rospy.get_published_topics()] 109 | self.assertNotIn('/test/inputE', topics) 110 | self.assertNotIn('/test/outputE', topics) 111 | print(topics) 112 | 113 | def test_manager_unstamped(self): 114 | rate = rospy.Rate(100) 115 | 116 | msg = self.generate_twist_stamped(rospy.Time.now()) 117 | self.pub.publish(msg) 118 | 119 | for timeout in range(1000): 120 | if self.message_received: 121 | break 122 | rate.sleep() 123 | 124 | self.assertEqual(msg.twist, self.msg) 125 | 126 | def test_manager_stamped(self): 127 | TEST = 'test_manager_stamped' 128 | rate = rospy.Rate(100) 129 | 130 | msg = self.generate_twist_stamped(rospy.Time.now(), frame_id=TEST) 131 | self.pub.publish(msg) 132 | 133 | for timeout in range(1000): 134 | if self.message_received: 135 | break 136 | rate.sleep() 137 | 138 | self.assertTrue(self.twist_stamped_eq(msg, self.msg)) 139 | 140 | def test_latency_management_zero(self): 141 | rate = rospy.Rate(100) 142 | msg = self.generate_twist_stamped(rospy.Time.now()) 143 | 144 | # This is used to simulate latency 145 | rospy.sleep(10) 146 | 147 | self.pub.publish(msg) 148 | 149 | for timeout in range(1000): 150 | if self.message_received: 151 | break 152 | rate.sleep() 153 | 154 | self.assertEqual(msg.twist, self.msg) 155 | 156 | def test_latency_management_one(self): 157 | rate = rospy.Rate(100) 158 | msg = self.generate_twist_stamped(rospy.Time.now()) 159 | 160 | # This is used to simulate latency 161 | rospy.sleep(10) 162 | 163 | self.pub.publish(msg) 164 | 165 | for timeout in range(1000): 166 | if self.message_received: 167 | break 168 | rate.sleep() 169 | 170 | self.assertEqual(None, self.msg) 171 | 172 | def test_latency_management_less_than_zero(self): 173 | rate = rospy.Rate(100) 174 | msg = self.generate_twist_stamped(rospy.Time.now()) 175 | 176 | # This is used to simulate latency 177 | rospy.sleep(10) 178 | 179 | self.pub.publish(msg) 180 | 181 | for timeout in range(1000): 182 | if self.message_received: 183 | break 184 | rate.sleep() 185 | 186 | self.assertEqual(msg.twist, self.msg) 187 | 188 | 189 | rospy.init_node('test_control_input_manager_functional_test') 190 | rosunit.unitrun(PKG, 'test_control_input_manager_functional', TestTwistCommand) 191 | -------------------------------------------------------------------------------- /roverrobotics_input_manager/test/test_latency.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | import unittest 4 | import rosunit 5 | from geometry_msgs.msg import Twist, TwistStamped 6 | import numpy as np 7 | 8 | PKG = 'roverrobotics_input_manager' 9 | 10 | 11 | class TestLatency(unittest.TestCase): 12 | def __init__(self, *args, **kwargs): 13 | super(TestLatency, self).__init__(*args, **kwargs) 14 | self.trials = 1000 15 | self.message_received = False 16 | self.message_stamp = None 17 | self.msg_latency = np.zeros(self.trials) 18 | self.msg = None 19 | self.pub = None 20 | self.sub = None 21 | 22 | def setUp(self): 23 | test_method_name = self._testMethodName 24 | subscribers = {'test_twist_unstamped_latency': ('/test/outputA', Twist), 25 | 'test_twist_stamped_latency': ('/test/outputF', TwistStamped) 26 | } 27 | publishers = {'test_twist_unstamped_latency': ('/test/inputA', TwistStamped), 28 | 'test_twist_stamped_latency': ('/test/inputF', TwistStamped), 29 | } 30 | 31 | if test_method_name in subscribers.keys(): 32 | self.sub = rospy.Subscriber(subscribers[test_method_name][0], 33 | subscribers[test_method_name][1], 34 | self.listener, 35 | queue_size=10) 36 | if test_method_name in publishers.keys(): 37 | self.pub = rospy.Publisher(publishers[test_method_name][0], 38 | publishers[test_method_name][1], 39 | queue_size=1) 40 | 41 | # Allow time for the publisher and subscriber to properly connect to endpoints 42 | rospy.sleep(1) 43 | 44 | def tearDown(self): 45 | if self.sub: 46 | self.sub.unregister() 47 | if self.pub: 48 | self.pub.unregister() 49 | 50 | def listener(self, data): 51 | self.message_stamp = rospy.Time.now() 52 | self.message_received = True 53 | self.msg = data 54 | 55 | def reset_message_info(self): 56 | self.msg = None 57 | self.message_received = False 58 | self.message_stamp = None 59 | 60 | def generate_twist(self, x=0, y=0, z=0, rx=0, ry=0, rz=0): 61 | msg = Twist() 62 | msg.linear.x = x 63 | msg.linear.y = y 64 | msg.linear.z = z 65 | msg.angular.x = rx 66 | msg.angular.y = ry 67 | msg.angular.z = rz 68 | 69 | return msg 70 | 71 | def generate_twist_stamped(self, time, seq=0, frame_id='', x=0, y=0, z=0, ax=0, ay=0, az=0): 72 | msg = TwistStamped() 73 | msg.header.seq = seq 74 | msg.header.stamp = time 75 | msg.header.frame_id = frame_id 76 | msg.twist.linear.x = x 77 | msg.twist.linear.y = y 78 | msg.twist.linear.z = z 79 | msg.twist.angular.x = ax 80 | msg.twist.angular.y = ay 81 | msg.twist.angular.z = az 82 | 83 | return msg 84 | 85 | def compare_twist_stamped(self, msg1, msg2): 86 | result = msg1.header.stamp == msg2.header.stamp \ 87 | and msg1.header.frame_id == msg2.header.frame_id \ 88 | and msg1.twist == msg2.twist 89 | 90 | return result 91 | 92 | def test_twist_stamped_latency(self): 93 | rate = rospy.Rate(100) 94 | 95 | for i in range(self.trials): 96 | msg = self.generate_twist_stamped(rospy.Time.now(), frame_id=str(i)) 97 | self.pub.publish(msg) 98 | 99 | for timeout in range(1000): 100 | if self.message_received: 101 | break 102 | rate.sleep() 103 | 104 | self.assertTrue(self.compare_twist_stamped(msg, self.msg)) 105 | self.msg_latency[i] = (self.message_stamp - msg.header.stamp).to_sec() 106 | self.reset_message_info() 107 | 108 | self.assertGreater(np.min(self.msg_latency), 0.0) 109 | self.assertLess(np.mean(self.msg_latency), 0.01) 110 | self.assertLess(np.max(self.msg_latency), 0.05) 111 | 112 | def test_twist_unstamped_latency(self): 113 | rate = rospy.Rate(100) 114 | 115 | for i in range(self.trials): 116 | msg = self.generate_twist_stamped(rospy.Time.now(), frame_id=str(i)) 117 | self.pub.publish(msg) 118 | 119 | for timeout in range(1000): 120 | if self.message_received: 121 | break 122 | rate.sleep() 123 | 124 | self.assertEqual(msg.twist, self.msg) 125 | self.msg_latency[i] = (self.message_stamp - msg.header.stamp).to_sec() 126 | self.reset_message_info() 127 | 128 | self.assertGreater(np.min(self.msg_latency), 0.0) 129 | self.assertLess(np.mean(self.msg_latency), 0.01) 130 | self.assertLess(np.max(self.msg_latency), 0.05) 131 | 132 | 133 | rospy.init_node('test_control_input_manager_latency_test') 134 | rosunit.unitrun(PKG, 'test_control_input_manager_latency', TestLatency) 135 | -------------------------------------------------------------------------------- /roverrobotics_simulation/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package roverrobotics_simulation 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 2.0.1 (2020-12-14) 5 | ------------------ 6 | 7 | 2.0.0 (2020-11-10) 8 | ------------------ 9 | 10 | 1.2.0 (2020-11-05) 11 | ------------------ 12 | 13 | 1.1.1 (2020-06-15) 14 | ------------------ 15 | * add pyserial to package.xml 16 | * Contributors: padiln 17 | 18 | 1.1.0 (2020-06-08) 19 | ------------------ 20 | * release to melodic 21 | 22 | 1.0.0 (2020-06-08) 23 | ------------------ 24 | * update simulation package version 25 | * Merge pull request `#69 `_ from RoverRobotics/feature/rook/gazebo-xcontroller 26 | Xbox controller input for Gazebo simulation 27 | * clean up launch files 28 | * fix file reference 29 | * ungroup launch files 30 | * Xbox controller input for Gazebo simulation 31 | * add world_name args to gazebo launch files 32 | * cleanup simulation launch files 33 | * adding rviz and gazebo simulation to rr_openrover stack 34 | * added support for gazebo and rviz simulation for 2wd and 4wd rovers 35 | * Contributors: William Rook, padiln 36 | 37 | 0.8.0 (2020-03-25) 38 | ------------------ 39 | 40 | 0.7.4 (2020-02-19) 41 | ------------------ 42 | 43 | 0.7.3 (2019-10-14) 44 | ------------------ 45 | 46 | 0.7.2 (2019-07-18) 47 | ------------------ 48 | -------------------------------------------------------------------------------- /roverrobotics_simulation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(roverrobotics_simulation) 3 | 4 | find_package(catkin REQUIRED) 5 | catkin_package( 6 | # INCLUDE_DIRS include 7 | # LIBRARIES roverrobotics_simulation 8 | # CATKIN_DEPENDS other_catkin_pkg 9 | # DEPENDS system_lib 10 | ) 11 | 12 | install(DIRECTORY launch/ 13 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 14 | FILES_MATCHING PATTERN "*.launch" 15 | ) -------------------------------------------------------------------------------- /roverrobotics_simulation/launch/gazebo/2wd_rover_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 26 | 27 | 28 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /roverrobotics_simulation/launch/gazebo/2wd_rover_gazebo_teleop.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 | -------------------------------------------------------------------------------- /roverrobotics_simulation/launch/gazebo/4wd_rover_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 26 | 27 | 28 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /roverrobotics_simulation/launch/gazebo/4wd_rover_gazebo_teleop.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 | -------------------------------------------------------------------------------- /roverrobotics_simulation/launch/rviz/2wd_rover_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /roverrobotics_simulation/launch/rviz/4wd_rover_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /roverrobotics_simulation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | roverrobotics_simulation 4 | 2.0.1 5 | The roverrobotics_simulation package 6 | 7 | 8 | 9 | 10 | John Bartholomew 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | catkin 19 | std_msgs 20 | gazebo_msgs 21 | gazebo_plugins 22 | gazebo_ros 23 | gazebo_ros_control 24 | gazebo_ros_pkgs 25 | teleop_twist_joy 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | --------------------------------------------------------------------------------