├── .clang-format ├── .gitignore ├── .travis.yml ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake └── FindGflags.cmake ├── include └── ros_control_boilerplate │ ├── generic_hw_control_loop.h │ ├── generic_hw_interface.h │ ├── sim_hw_interface.h │ └── tools │ ├── controller_to_csv.h │ ├── csv_to_controller.h │ └── joystick_manual_control.h ├── package.xml ├── resources └── screenshot.png ├── rrbot_control ├── CMakeLists.txt ├── config │ └── rrbot_controllers.yaml ├── include │ └── rrbot_control │ │ └── rrbot_hw_interface.h ├── launch │ ├── rrbot_hardware.launch │ ├── rrbot_simulation.launch │ ├── rrbot_test_trajectory.launch │ └── rrbot_visualize.launch ├── package.xml └── src │ ├── rrbot_hw_interface.cpp │ └── rrbot_hw_main.cpp ├── scripts └── analyze_trajectory.m └── src ├── generic_hw_control_loop.cpp ├── generic_hw_interface.cpp ├── sim_hw_interface.cpp ├── sim_hw_main.cpp └── tools ├── controller_to_csv.cpp ├── controller_to_csv_main.cpp ├── csv_to_controller.cpp ├── csv_to_controller_main.cpp ├── keyboard_teleop.cpp └── test_trajectory.cpp /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | ColumnLimit: 120 4 | MaxEmptyLinesToKeep: 1 5 | SortIncludes: false 6 | 7 | Standard: Auto 8 | IndentWidth: 2 9 | TabWidth: 2 10 | UseTab: Never 11 | AccessModifierOffset: -2 12 | ConstructorInitializerIndentWidth: 2 13 | NamespaceIndentation: None 14 | ContinuationIndentWidth: 4 15 | IndentCaseLabels: true 16 | IndentFunctionDeclarationAfterType: false 17 | 18 | AlignEscapedNewlinesLeft: false 19 | AlignTrailingComments: true 20 | 21 | AllowAllParametersOfDeclarationOnNextLine: false 22 | ExperimentalAutoDetectBinPacking: false 23 | ObjCSpaceBeforeProtocolList: true 24 | Cpp11BracedListStyle: false 25 | 26 | AllowShortBlocksOnASingleLine: true 27 | AllowShortIfStatementsOnASingleLine: false 28 | AllowShortLoopsOnASingleLine: false 29 | AllowShortFunctionsOnASingleLine: None 30 | AllowShortCaseLabelsOnASingleLine: false 31 | 32 | AlwaysBreakTemplateDeclarations: true 33 | AlwaysBreakBeforeMultilineStrings: false 34 | BreakBeforeBinaryOperators: false 35 | BreakBeforeTernaryOperators: false 36 | BreakConstructorInitializersBeforeComma: true 37 | 38 | BinPackParameters: true 39 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 40 | DerivePointerBinding: false 41 | PointerBindsToType: true 42 | 43 | PenaltyExcessCharacter: 50 44 | PenaltyBreakBeforeFirstCallParameter: 30 45 | PenaltyBreakComment: 1000 46 | PenaltyBreakFirstLessLess: 10 47 | PenaltyBreakString: 100 48 | PenaltyReturnTypeOnItsOwnLine: 50 49 | 50 | SpacesBeforeTrailingComments: 2 51 | SpacesInParentheses: false 52 | SpacesInAngles: false 53 | SpaceInEmptyParentheses: false 54 | SpacesInCStyleCastParentheses: false 55 | SpaceAfterCStyleCast: false 56 | SpaceAfterControlStatementKeyword: true 57 | SpaceBeforeAssignmentOperators: true 58 | 59 | # Configure each individual brace in BraceWrapping 60 | BreakBeforeBraces: Custom 61 | 62 | # Control of individual brace wrapping cases 63 | BraceWrapping: 64 | AfterCaseLabel: true 65 | AfterClass: true 66 | AfterControlStatement: true 67 | AfterEnum: true 68 | AfterFunction: true 69 | AfterNamespace: true 70 | AfterStruct: true 71 | AfterUnion: true 72 | BeforeCatch: true 73 | BeforeElse: true 74 | IndentBraces: false 75 | ... 76 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | 19 | # Compiled Static libraries 20 | *.lai 21 | *.la 22 | *.a 23 | *.lib 24 | 25 | # Executables 26 | *.exe 27 | *.out 28 | *.app 29 | 30 | # Matlab 31 | *~ -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # This config file for Travis CI utilizes https://github.com/ros-planning/moveit_ci package. 2 | sudo: required 3 | dist: xenial # distro used by Travis, moveit_ci uses the docker image's distro 4 | services: 5 | - docker 6 | language: cpp 7 | compiler: gcc 8 | cache: ccache 9 | 10 | env: 11 | matrix: 12 | - ROS_DISTRO=noetic 13 | - ROS_DISTRO=noetic TEST="clang-format catkin_lint" 14 | 15 | before_script: 16 | - git clone -q --depth=1 https://github.com/ros-planning/moveit_ci.git .moveit_ci 17 | 18 | script: 19 | - .moveit_ci/travis.sh 20 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package ros_control_boilerplate 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.6.1 (2021-02-23) 6 | ------------------ 7 | * Increase required cmake version to fix CMake CMP0048 warning 8 | * Replaced boost with std shared_ptr 9 | * Make NodeHandle a const reference 10 | * test_trajectory: Read joints list from trajectory controller params 11 | * Increase num AsyncSpinners where control loops are instantiated 12 | * Contributors: AndyZe, Dave Coleman, John Morris, Ramon Wijnands, Robert Wilbrandt, Tim Übelhör 13 | 14 | 0.6.0 (2020-06-16) 15 | ------------------ 16 | * Generalize GenericHWControlLoop to all types of RobotHW (`#38 `_) 17 | * Update README.md 18 | * Fix build badges in README (`#37 `_) 19 | * Update .travis.yml to use moveit_ci (`#36 `_) 20 | * Contributors: Jafar Abdi, RobertWilbrandt 21 | 22 | 0.5.0 (2019-09-18) 23 | ------------------ 24 | * Merge pull request `#20 `_ from ipa-mdl/fix-loop-deadlock 25 | refactor GenericHWControlLoop to a sleep-based loop 26 | * Merge pull request `#19 `_ from PaulBouchier/kinetic-devel 27 | change sim_control_mode to 0 (position) so demo works 28 | * Merge pull request `#21 `_ from MohmadAyman/fix_typo 29 | fixed a typo in readme 30 | * fixed a typo in readme 31 | * refactor GenericHWControlLoop to a sleep-based loop 32 | using ros:Timer might lead to deadlocks 33 | * initialize desired_update_period\_ (renamed from desired_update_freq\_) 34 | * Revert "Depend on Eigen3" 35 | This reverts commit 608cc2fd64739ee56c3fbd5a0ae9d5d26b5684d0. 36 | * change sim_control_mode to 0 (position) so demo works 37 | * Merge pull request `#16 `_ from lucasw/xml-version 38 | xml version tags for all launch files. 39 | * xml version tags for all launch files. 40 | * Merge pull request `#15 `_ from enricotoi/kinetic-devel 41 | Fixed a typo in the README.md 42 | * Fixed a typo in the README.md 43 | rrbot_simulaton.launch -> rrbot_simulation.launch 44 | * Update README.md 45 | * Contributors: Dave Coleman, Lucas Walter, Mathias Lüdtke, Paul Bouchier, enrico toivinen, mohmad ayman 46 | 47 | 0.4.1 (2017-06-20) 48 | ------------------ 49 | * Changed boost::shared_ptr to typedef for Lunar support 50 | * Implemented simulated velocity control 51 | * Contributors: Dave Coleman 52 | 53 | 0.4.0 (2016-06-29) 54 | ------------------ 55 | * Depend on Eigen3 56 | * Remove dependency on meta package 57 | * Fixed var name 58 | * Contributors: Dave Coleman 59 | 60 | 0.3.1 (2016-01-13) 61 | ------------------ 62 | * API deprecation fix for rosparam_shortcuts 63 | * Switched to better use of rosparam_shortcuts 64 | * Ability to record all controller status data, not just at certain frequency 65 | * Contributors: Dave Coleman 66 | 67 | 0.3.0 (2015-12-27) 68 | ------------------ 69 | * Removed bad reference name 70 | * Switched to using name\_ 71 | * Record error data 72 | * Disable soft joint limits 73 | * header to debug output 74 | * Added error checking of control loops time 75 | * Fix init() bug 76 | * Contributors: Dave Coleman 77 | 78 | 0.2.1 (2015-12-09) 79 | ------------------ 80 | * Merge branch 'indigo-devel' of github.com:davetcoleman/ros_control_boilerplate into indigo-devel 81 | * Fix install path 82 | * Improve user output message 83 | * Contributors: Dave Coleman 84 | 85 | 0.2.0 (2015-12-09) 86 | ------------------ 87 | * Do not automatically call init() 88 | * Removed warning of joint limits for continous joints 89 | * Fix missing variable 90 | * Improved rrbot_control example package 91 | * Moved rrbot example code into subdirectory 92 | * Contributors: Dave Coleman 93 | 94 | 0.1.4 (2015-12-07) 95 | ------------------ 96 | * Added missing dependency on sensor_msgs 97 | * Contributors: Dave Coleman 98 | 99 | 0.1.3 (2015-12-05) 100 | ------------------ 101 | * Fix catkin lint errors 102 | * Added FindGflags directly to this repo 103 | * Minor fix 104 | * Updated README 105 | * Contributors: Dave Coleman 106 | 107 | 0.1.2 (2015-12-02) 108 | ------------------ 109 | * Added dependency on gflags 110 | * Contributors: Dave Coleman 111 | 112 | 0.1.1 (2015-12-02) 113 | ------------------ 114 | * Added travis support 115 | * Updated README 116 | * Contributors: Dave Coleman 117 | 118 | 0.1.0 (2015-12-02) 119 | ------------------ 120 | * Initial release of ros_control_boilerplate 121 | * Contributors: Dave Coleman 122 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(ros_control_boilerplate) 3 | 4 | # C++ 11 5 | if(NOT "${CMAKE_CXX_STANDARD}") 6 | set(CMAKE_CXX_STANDARD 11) 7 | endif() 8 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 9 | set(CMAKE_CXX_EXTENSIONS OFF) 10 | 11 | find_package(catkin REQUIRED COMPONENTS 12 | actionlib 13 | cmake_modules 14 | control_msgs 15 | control_toolbox 16 | controller_manager 17 | hardware_interface 18 | joint_limits_interface 19 | roscpp 20 | rosparam_shortcuts 21 | sensor_msgs 22 | std_msgs 23 | trajectory_msgs 24 | transmission_interface 25 | urdf 26 | ) 27 | 28 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/cmake) 29 | find_package(Gflags REQUIRED) 30 | 31 | catkin_package( 32 | INCLUDE_DIRS 33 | include 34 | CATKIN_DEPENDS 35 | actionlib 36 | control_msgs 37 | control_toolbox 38 | controller_manager 39 | hardware_interface 40 | joint_limits_interface 41 | roscpp 42 | rosparam_shortcuts 43 | sensor_msgs 44 | std_msgs 45 | trajectory_msgs 46 | transmission_interface 47 | urdf 48 | LIBRARIES 49 | generic_hw_control_loop 50 | generic_hw_interface 51 | sim_hw_interface 52 | ) 53 | 54 | ########### 55 | ## Build ## 56 | ########### 57 | 58 | include_directories( 59 | include/ 60 | ${catkin_INCLUDE_DIRS} 61 | ${Gflags_INCLUDE_DIRS} 62 | ) 63 | 64 | # Control loop 65 | add_library(generic_hw_control_loop 66 | src/generic_hw_control_loop.cpp 67 | ) 68 | target_link_libraries(generic_hw_control_loop 69 | ${catkin_LIBRARIES} 70 | ) 71 | 72 | # Generic Hardware Interface 73 | add_library(generic_hw_interface 74 | src/generic_hw_interface.cpp 75 | ) 76 | target_link_libraries(generic_hw_interface 77 | ${catkin_LIBRARIES} 78 | ) 79 | 80 | # Simulation Hardware Interface 81 | add_library(sim_hw_interface 82 | src/sim_hw_interface.cpp 83 | ) 84 | target_link_libraries(sim_hw_interface 85 | generic_hw_interface 86 | ${catkin_LIBRARIES} 87 | ) 88 | 89 | # Main control executable 90 | add_executable(${PROJECT_NAME}_sim_hw_main src/sim_hw_main.cpp) 91 | set_target_properties(${PROJECT_NAME}_sim_hw_main PROPERTIES OUTPUT_NAME sim_hw_main PREFIX "") 92 | target_link_libraries(${PROJECT_NAME}_sim_hw_main 93 | sim_hw_interface 94 | generic_hw_control_loop 95 | ${catkin_LIBRARIES} 96 | ) 97 | 98 | # Test trajectory generator node 99 | add_executable(${PROJECT_NAME}_test_trajectory src/tools/test_trajectory.cpp) 100 | set_target_properties(${PROJECT_NAME}_test_trajectory PROPERTIES OUTPUT_NAME test_trajectory PREFIX "") 101 | target_link_libraries(${PROJECT_NAME}_test_trajectory 102 | ${catkin_LIBRARIES} 103 | ) 104 | 105 | ## TOOLS ------------------------------------------------------ 106 | 107 | # Tool for analyzing controller performance 108 | add_library(controller_to_csv src/tools/controller_to_csv.cpp) 109 | target_link_libraries(controller_to_csv 110 | ${catkin_LIBRARIES} 111 | ) 112 | add_executable(${PROJECT_NAME}_controller_to_csv_main src/tools/controller_to_csv_main.cpp) 113 | set_target_properties(${PROJECT_NAME}_controller_to_csv_main PROPERTIES OUTPUT_NAME controller_to_csv_main PREFIX "") 114 | target_link_libraries(${PROJECT_NAME}_controller_to_csv_main 115 | controller_to_csv 116 | ${Gflags_LIBRARIES} 117 | ${catkin_LIBRARIES} 118 | ) 119 | 120 | # Tool for analyzing controller performance 121 | add_library(csv_to_controller src/tools/csv_to_controller.cpp) 122 | target_link_libraries(csv_to_controller 123 | ${catkin_LIBRARIES} 124 | ) 125 | add_executable(${PROJECT_NAME}_csv_to_controller_main src/tools/csv_to_controller_main.cpp) 126 | set_target_properties(${PROJECT_NAME}_csv_to_controller_main PROPERTIES OUTPUT_NAME csv_to_controller_main PREFIX "") 127 | target_link_libraries(${PROJECT_NAME}_csv_to_controller_main 128 | csv_to_controller 129 | ${Gflags_LIBRARIES} 130 | ${catkin_LIBRARIES} 131 | ) 132 | 133 | # Tool for controlling a robot from keyboard 134 | add_executable(${PROJECT_NAME}_keyboard_teleop src/tools/keyboard_teleop.cpp) 135 | set_target_properties(${PROJECT_NAME}_keyboard_teleop PROPERTIES OUTPUT_NAME keyboard_teleop PREFIX "") 136 | target_link_libraries(${PROJECT_NAME}_keyboard_teleop 137 | ${catkin_LIBRARIES} 138 | ) 139 | 140 | add_subdirectory(rrbot_control) 141 | 142 | ## Install ------------------------------------------------------------ 143 | 144 | # Install libraries 145 | install(TARGETS 146 | controller_to_csv 147 | csv_to_controller 148 | generic_hw_control_loop 149 | generic_hw_interface 150 | sim_hw_interface 151 | LIBRARY DESTINATION 152 | ${CATKIN_PACKAGE_LIB_DESTINATION} 153 | ) 154 | 155 | # Install executables 156 | install(TARGETS 157 | ${PROJECT_NAME}_controller_to_csv_main 158 | ${PROJECT_NAME}_csv_to_controller_main 159 | ${PROJECT_NAME}_keyboard_teleop 160 | ${PROJECT_NAME}_sim_hw_main 161 | ${PROJECT_NAME}_test_trajectory 162 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 163 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 164 | ) 165 | 166 | # Install header files 167 | install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 168 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2015, Dave Coleman 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of ros_control_boilerplate nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ROS Control Boilerplate 2 | 3 | Simple simulation interface and template for setting up a hardware interface for ros_control. The idea is you take this as a starting point for creating your hardware interfaces, and it is needed because [ros_control documentation](http://wiki.ros.org/ros_control) is sparse. This boilerplate demonstrates: 4 | 5 | - Creating a hardware_interface for multiple joints for use with ros_control 6 | - Position Trajectory Controller 7 | - Control of 2 joints of the simple robot "RRBot" pictured below 8 | - Loading configurations with roslaunch and yaml files 9 | - Generating a random trajectory and sending it over an actionlib interface 10 | - Partial support of joint mode switching (needs to be improved) 11 | - Joint limits 12 | - Pass-through non-physics based robot simulator 13 | - Visualization in Rviz 14 | 15 | 16 | 17 | This open source project was developed at [PickNik Robotics](https://picknik.ai/). Need professional ROS development and consulting? Contact us at projects@picknik.ai for a free consultation. 18 | 19 | ## Maintainers 20 | 21 | Special thanks to the following maintainers of this repo: 22 | 23 | - Dave Coleman (@davetcoleman) 24 | - Andy Zelenak (@AndyZe) 25 | - John Morris (@zultron) 26 | - Robert Wilbrandt (@RobertWilbrandt) 27 | 28 | ## Status: 29 | 30 | * [![Build Status](https://travis-ci.org/PickNikRobotics/ros_control_boilerplate.svg?branch=melodic-devel)](https://travis-ci.org/PickNikRobotics/ros_control_boilerplate) Travis CI 31 | * [![Devel Job Status](http://build.ros.org/buildStatus/icon?job=Mdev__ros_control_boilerplate__ubuntu_bionic_amd64)](http://build.ros.org/job/Mdev__ros_control_boilerplate__ubuntu_bionic_amd64/) Devel Job Status 32 | * [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__ros_control_boilerplate__ubuntu_bionic_amd64__binary)](http://build.ros.org/job/Mbin_uB64__ros_control_boilerplate__ubuntu_bionic_amd64__binary/) AMD64 Debian Job Status 33 | 34 | 35 | 36 | 37 | ## Video Demo 38 | 39 | See [YouTube](https://www.youtube.com/watch?v=Tpj2tx9uZ-o) for a very modest video demo. 40 | 41 | ## Install 42 | 43 | This package depends on [gazebo_ros_demos](https://github.com/ros-simulation/gazebo_ros_demos) for its ``rrbot_description`` package, but you must add it to your catkin workspace by source: 44 | 45 | git clone https://github.com/ros-simulation/gazebo_ros_demos.git 46 | 47 | Then, either install this package from source so you can develop off of it, or install from debian: 48 | 49 | sudo apt-get install ros-indigo-ros-control-boilerplate 50 | 51 | ## Run Simulation Demo 52 | 53 | This package is setup to run the "RRBot" two joint revolute-revolute robot demo. This "template package" is located in the ros_control_boilerplate as a subfolder that you can easily rename and reuse. To run its ros_control non-physics-based simulated hardware interface, run: 54 | 55 | roslaunch ros_control_boilerplate rrbot_simulation.launch 56 | 57 | To visualize its published ``/tf`` coordinate transforms in Rviz run: 58 | 59 | roslaunch ros_control_boilerplate rrbot_visualize.launch 60 | 61 | To send a random, dummy trajectory to execute, run: 62 | 63 | roslaunch ros_control_boilerplate rrbot_test_trajectory.launch 64 | 65 | ## Customize 66 | 67 | To test this as a simulation interface for your robot, you can quickly rename the subfolder package into the name of your robot using the following commands: 68 | 69 | ``` 70 | function findreplace() { 71 | grep -lr -e "$1" * | xargs sed -i "s/$1/$2/g" ; 72 | } 73 | 74 | function findreplacefilename() { 75 | find . -depth -name "*$1*" -exec bash -c 'for f; do base=${f##*/}; mv -- "$f" "${f%/*}/${base//'$1'/'$2'}"; done' _ {} + 76 | } 77 | 78 | findreplacefilename rrbot myrobot 79 | findreplace rrbot myrobot 80 | findreplace RRBot MyRobot 81 | findreplace RRBOT MYROBOT 82 | ``` 83 | 84 | Then add the necessary code to communicate with your robot via USB/serial/ethernet/etc in the file ``myrobot_hw_interface.cpp``. 85 | 86 | ## Setting an Initial Position, Using with MoveIt! 87 | 88 | If you need your robot to startup at a particular position in simulation, or you would like to use this funcitonality to simulate your robot with MoveIt!, see the downstream package (it depends on this package) [moveit_sim_controller](https://github.com/davetcoleman/moveit_sim_controller) 89 | 90 | ## Other Helper Tools 91 | 92 | ### Recording to CSV 93 | 94 | Write the commands from a trajectory controller to csv file 95 | 96 | rosrun ros_control_boilerplate controller_to_csv SAVE_TO_FILE_PATH CONTROLLER_STATE_TOPIC TIME_TO_RECORD 97 | 98 | ### Commanding from CSV 99 | 100 | Read from csv file and execute on robot 101 | 102 | rosrun ros_control_boilerplate csv_to_controller READ_FROM_FILE_PATH CONTROLLER_STATE_TOPIC TIME_TO_RECORD 103 | 104 | ### Commanding from Keyboard 105 | 106 | Joint-level teleop from a keyboard (TODO: remove had coded topic names) 107 | 108 | rosrun ros_control_boilerplate keyboard_teleop 109 | 110 | ## Limitations 111 | 112 | - Does not implement estops, transmissions, or other fancy new features of ros_control 113 | - Does not have any hard realtime code, this depends largely on your platform, kernel, OS, etc 114 | - Only position control is fully implemented, though some code is in place for velocity control 115 | 116 | ## Contribute 117 | 118 | Please add features, make corrections, and address the limitations above, thanks! 119 | -------------------------------------------------------------------------------- /cmake/FindGflags.cmake: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # 3 | # CMake script for finding the GFlags library. 4 | # 5 | # https://gflags.github.io/gflags/ 6 | # 7 | # Redistribution and use is allowed according to the terms of the 3-clause BSD 8 | # license. 9 | # 10 | # Input variables: 11 | # 12 | # - Gflags_FIND_REQUIRED: throws an error if gflags is not found but it is required 13 | # 14 | # Output variables: 15 | # 16 | # - Gflags_FOUND: Boolean that indicates if the package was found 17 | # - Gflags_LIBRARIES: Package libraries 18 | # - Gflags_INCLUDE_DIRS: Absolute path to package headers 19 | # 20 | # Example usage: 21 | # 22 | # find_package(Gflags REQUIRED) 23 | # 24 | # include_directories( 25 | # ${Gflags_INCLUDE_DIRS} 26 | # ) 27 | # 28 | # add_executable(main src/main.cpp) 29 | # target_link_libraries(main 30 | # ${Gflags_LIBRARIES} 31 | # ) 32 | ############################################################################### 33 | 34 | 35 | find_path(Gflags_INCLUDE_PATH gflags/gflags.h) 36 | 37 | find_library(Gflags_LIBRARY NAMES gflags libgflags) 38 | 39 | if(Gflags_INCLUDE_PATH AND Gflags_LIBRARY) 40 | set(Gflags_FOUND TRUE) 41 | endif(Gflags_INCLUDE_PATH AND Gflags_LIBRARY) 42 | 43 | if(Gflags_FOUND) 44 | message(STATUS "Found gflags: ${Gflags_LIBRARY}") 45 | # Output variables 46 | set(Gflags_INCLUDE_DIRS ${Gflags_INCLUDE_DIR}) 47 | set(Gflags_LIBRARIES ${Gflags_LIBRARY}) 48 | else(Gflags_FOUND) 49 | if(Gflags_FIND_REQUIRED) 50 | message(FATAL_ERROR "Could not find gflags library.") 51 | endif(Gflags_FIND_REQUIRED) 52 | endif(Gflags_FOUND) 53 | -------------------------------------------------------------------------------- /include/ros_control_boilerplate/generic_hw_control_loop.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Example control loop for reading, updating, and writing commands to a hardware interface 37 | using MONOTOIC system time 38 | */ 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | namespace ros_control_boilerplate 45 | { 46 | // Used to convert seconds elapsed to nanoseconds 47 | static const double BILLION = 1000000000.0; 48 | 49 | /** 50 | * \brief The control loop - repeatidly calls read() and write() to the hardware interface at a 51 | * specified frequency 52 | * We use MONOTONIC time to ensure robustness in the event of system time updates/change. 53 | * See 54 | * http://stackoverflow.com/questions/3523442/difference-between-clock-realtime-and-clock-monotonic 55 | */ 56 | class GenericHWControlLoop 57 | { 58 | public: 59 | /** 60 | * \brief Constructor 61 | * \param NodeHandle 62 | * \param hardware_interface - the robot-specific hardware interface to be use with your robot 63 | */ 64 | GenericHWControlLoop(ros::NodeHandle& nh, std::shared_ptr hardware_interface); 65 | 66 | // Run the control loop (blocking) 67 | void run(); 68 | 69 | protected: 70 | // Update funcion called with loop_hz_ rate 71 | void update(); 72 | 73 | // Startup and shutdown of the internal node inside a roscpp program 74 | ros::NodeHandle nh_; 75 | 76 | // Name of this class 77 | std::string name_ = "generic_hw_control_loop"; 78 | 79 | // Settings 80 | ros::Duration desired_update_period_; 81 | double cycle_time_error_threshold_; 82 | 83 | // Timing 84 | ros::Duration elapsed_time_; 85 | double loop_hz_; 86 | struct timespec last_time_; 87 | struct timespec current_time_; 88 | 89 | /** \brief ROS Controller Manager and Runner 90 | * 91 | * This class advertises a ROS interface for loading, unloading, starting, and 92 | * stopping ros_control-based controllers. It also serializes execution of all 93 | * running controllers in \ref update. 94 | */ 95 | std::shared_ptr controller_manager_; 96 | 97 | /** \brief Abstract Hardware Interface for your robot */ 98 | std::shared_ptr hardware_interface_; 99 | 100 | }; // end class 101 | 102 | } // namespace ros_control_boilerplate 103 | -------------------------------------------------------------------------------- /include/ros_control_boilerplate/generic_hw_interface.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Example ros_control hardware interface that performs a perfect control loop for 37 | simulation 38 | */ 39 | 40 | #ifndef GENERIC_ROS_CONTROL_GENERIC_HW_INTERFACE_H 41 | #define GENERIC_ROS_CONTROL_GENERIC_HW_INTERFACE_H 42 | 43 | // ROS 44 | #include 45 | #include 46 | 47 | // ROS Controls 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | 57 | namespace ros_control_boilerplate 58 | { 59 | /// \brief Hardware interface for a robot 60 | class GenericHWInterface : public hardware_interface::RobotHW 61 | { 62 | public: 63 | /** 64 | * \brief Constructor 65 | * \param nh - Node handle for topics. 66 | * \param urdf - optional pointer to a parsed robot model 67 | */ 68 | GenericHWInterface(const ros::NodeHandle& nh, urdf::Model* urdf_model = NULL); 69 | 70 | /** \brief Destructor */ 71 | virtual ~GenericHWInterface() 72 | { 73 | } 74 | 75 | /** \brief Initialize the hardware interface */ 76 | virtual void init(); 77 | 78 | /** \brief Read the state from the robot hardware. */ 79 | virtual void read(ros::Duration& elapsed_time) = 0; 80 | 81 | /** \brief Read the state from the robot hardware 82 | * 83 | * \note This delegates RobotHW::read() calls to read() 84 | * 85 | * \param time The current time, currently unused 86 | * \param period The time passed since the last call 87 | */ 88 | virtual void read(const ros::Time& /*time*/, const ros::Duration& period) override 89 | { 90 | ros::Duration elapsed_time = period; 91 | read(elapsed_time); 92 | } 93 | 94 | /** \brief Write the command to the robot hardware. */ 95 | virtual void write(ros::Duration& elapsed_time) = 0; 96 | 97 | /** \brief Write the command to the robot hardware 98 | * 99 | * \note This delegates RobotHW::write() calls to \ref write() 100 | * 101 | * \param time The current time, currently unused 102 | * \param period The time passed since the last call 103 | */ 104 | virtual void write(const ros::Time& /*time*/, const ros::Duration& period) override 105 | { 106 | ros::Duration elapsed_time = period; 107 | write(elapsed_time); 108 | } 109 | 110 | /** \brief Set all members to default values */ 111 | virtual void reset(); 112 | 113 | /** 114 | * \brief Check (in non-realtime) if given controllers could be started and stopped from the 115 | * current state of the RobotHW 116 | * with regard to necessary hardware interface switches. Start and stop list are disjoint. 117 | * This is just a check, the actual switch is done in doSwitch() 118 | */ 119 | virtual bool canSwitch(const std::list& start_list, 120 | const std::list& stop_list) const 121 | { 122 | return true; 123 | } 124 | 125 | /** 126 | * \brief Perform (in non-realtime) all necessary hardware interface switches in order to start 127 | * and stop the given controllers. 128 | * Start and stop list are disjoint. The feasability was checked in canSwitch() beforehand. 129 | */ 130 | virtual void doSwitch(const std::list& start_list, 131 | const std::list& stop_list) 132 | { 133 | } 134 | 135 | /** 136 | * \brief Register the limits of the joint specified by joint_id and joint_handle. The limits 137 | * are retrieved from the urdf_model. 138 | * 139 | * \return the joint's type, lower position limit, upper position limit, and effort limit. 140 | */ 141 | virtual void registerJointLimits(const hardware_interface::JointHandle& joint_handle_position, 142 | const hardware_interface::JointHandle& joint_handle_velocity, 143 | const hardware_interface::JointHandle& joint_handle_effort, std::size_t joint_id); 144 | 145 | /** \breif Enforce limits for all values before writing */ 146 | virtual void enforceLimits(ros::Duration& period) = 0; 147 | 148 | /** \brief Helper for debugging a joint's state */ 149 | virtual void printState(); 150 | std::string printStateHelper(); 151 | 152 | /** \brief Helper for debugging a joint's command */ 153 | std::string printCommandHelper(); 154 | 155 | protected: 156 | /** \brief Get the URDF XML from the parameter server */ 157 | virtual void loadURDF(const ros::NodeHandle& nh, std::string param_name); 158 | 159 | // Short name of this class 160 | std::string name_; 161 | 162 | // Startup and shutdown of the internal node inside a roscpp program 163 | ros::NodeHandle nh_; 164 | 165 | // Hardware interfaces 166 | hardware_interface::JointStateInterface joint_state_interface_; 167 | hardware_interface::PositionJointInterface position_joint_interface_; 168 | hardware_interface::VelocityJointInterface velocity_joint_interface_; 169 | hardware_interface::EffortJointInterface effort_joint_interface_; 170 | 171 | // Joint limits interfaces - Saturation 172 | joint_limits_interface::PositionJointSaturationInterface pos_jnt_sat_interface_; 173 | joint_limits_interface::VelocityJointSaturationInterface vel_jnt_sat_interface_; 174 | joint_limits_interface::EffortJointSaturationInterface eff_jnt_sat_interface_; 175 | 176 | // Joint limits interfaces - Soft limits 177 | joint_limits_interface::PositionJointSoftLimitsInterface pos_jnt_soft_limits_; 178 | joint_limits_interface::VelocityJointSoftLimitsInterface vel_jnt_soft_limits_; 179 | joint_limits_interface::EffortJointSoftLimitsInterface eff_jnt_soft_limits_; 180 | 181 | // Configuration 182 | std::vector joint_names_; 183 | std::size_t num_joints_; 184 | urdf::Model* urdf_model_; 185 | 186 | // Modes 187 | bool use_rosparam_joint_limits_; 188 | bool use_soft_limits_if_available_; 189 | 190 | // States 191 | std::vector joint_position_; 192 | std::vector joint_velocity_; 193 | std::vector joint_effort_; 194 | 195 | // Commands 196 | std::vector joint_position_command_; 197 | std::vector joint_velocity_command_; 198 | std::vector joint_effort_command_; 199 | 200 | // Copy of limits, in case we need them later in our control stack 201 | std::vector joint_position_lower_limits_; 202 | std::vector joint_position_upper_limits_; 203 | std::vector joint_velocity_limits_; 204 | std::vector joint_effort_limits_; 205 | 206 | }; // class 207 | 208 | } // namespace ros_control_boilerplate 209 | 210 | #endif // GENERIC_ROS_CONTROL_GENERIC_HW_INTERFACE_H 211 | -------------------------------------------------------------------------------- /include/ros_control_boilerplate/sim_hw_interface.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Example ros_control hardware interface that performs a perfect control loop for 37 | simulation 38 | */ 39 | 40 | #ifndef GENERIC_ROS_CONTROL__SIM_HW_INTERFACE_H 41 | #define GENERIC_ROS_CONTROL__SIM_HW_INTERFACE_H 42 | 43 | #include 44 | 45 | namespace ros_control_boilerplate 46 | { 47 | /** \brief Hardware interface for a robot */ 48 | class SimHWInterface : public GenericHWInterface 49 | { 50 | public: 51 | /** 52 | * \brief Constructor 53 | * \param nh - Node handle for topics. 54 | */ 55 | SimHWInterface(ros::NodeHandle& nh, urdf::Model* urdf_model = NULL); 56 | 57 | /** \brief Initialize the robot hardware interface */ 58 | virtual void init(); 59 | 60 | /** \brief Read the state from the robot hardware. */ 61 | virtual void read(ros::Duration& elapsed_time); 62 | 63 | /** \brief Write the command to the robot hardware. */ 64 | virtual void write(ros::Duration& elapsed_time); 65 | 66 | /** \breif Enforce limits for all values before writing */ 67 | virtual void enforceLimits(ros::Duration& period); 68 | 69 | protected: 70 | /** \brief Basic model of system for position control */ 71 | virtual void positionControlSimulation(ros::Duration& elapsed_time, const std::size_t joint_id); 72 | 73 | // Name of this class 74 | std::string name_; 75 | 76 | // Simulated controller 77 | double p_error_; 78 | double v_error_; 79 | 80 | // For position controller to estimate velocity 81 | std::vector joint_position_prev_; 82 | 83 | // Send commands in different modes 84 | int sim_control_mode_ = 0; 85 | 86 | }; // class 87 | 88 | } // namespace ros_control_boilerplate 89 | 90 | #endif 91 | -------------------------------------------------------------------------------- /include/ros_control_boilerplate/tools/controller_to_csv.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Records a ros_control ControllerState data to CSV for Matlab/etc analysis 37 | */ 38 | 39 | #ifndef GENERIC_ROS_CONTROL__CONTROLLER_TO_CSV_H 40 | #define GENERIC_ROS_CONTROL__CONTROLLER_TO_CSV_H 41 | 42 | // ROS 43 | #include 44 | 45 | // ros_control 46 | #include 47 | 48 | namespace ros_control_boilerplate 49 | { 50 | class ControllerToCSV 51 | { 52 | public: 53 | /** 54 | * \brief Constructor 55 | * \param topic - ROS message to listen to from controller 56 | */ 57 | ControllerToCSV(const std::string& topic); 58 | 59 | /** \brief Destructor */ 60 | ~ControllerToCSV(); 61 | 62 | /** \brief Whether to record at a specific frequency, or record all incoming data */ 63 | bool recordAll(); 64 | 65 | /** \brief Start the data collection */ 66 | void startRecording(const std::string& file_name); 67 | 68 | /** \brief End recording */ 69 | void stopRecording(); 70 | 71 | private: 72 | /** \brief Send all resulting data to file */ 73 | bool writeToFile(); 74 | 75 | /** \brief Callback from ROS message */ 76 | void stateCB(const control_msgs::JointTrajectoryControllerState::ConstPtr& state); 77 | 78 | /** \brief Recieve data from controller via ROS message */ 79 | void update(const ros::TimerEvent& e); 80 | 81 | /** \brief Check if topic has been connected to successfully */ 82 | bool waitForSubscriber(const ros::Subscriber& sub, const double& wait_time = 10.0); 83 | 84 | // Class name 85 | std::string name_ = "controller_to_csv"; 86 | 87 | // A shared node handle 88 | ros::NodeHandle nh_; 89 | 90 | // Show status info on first update 91 | bool first_update_; 92 | bool recording_started_; 93 | 94 | // Listener to state of controller 95 | ros::Subscriber state_sub_; 96 | double record_hz_; // how often to record the latest incoming data. if zero, record all 97 | 98 | // Where to save the CSV 99 | std::string file_name_; 100 | 101 | // Buffer of controller state data 102 | std::vector states_; 103 | std::vector timestamps_; 104 | 105 | // Cache of last recieved state 106 | control_msgs::JointTrajectoryControllerState current_state_; 107 | 108 | // How often to sample the state 109 | ros::Timer non_realtime_loop_; 110 | 111 | }; // end class 112 | 113 | // Create std pointers for this class 114 | typedef std::shared_ptr ControllerToCSVPtr; 115 | typedef std::shared_ptr ControllerToCSVConstPtr; 116 | 117 | } // namespace ros_control_boilerplate 118 | 119 | #endif 120 | -------------------------------------------------------------------------------- /include/ros_control_boilerplate/tools/csv_to_controller.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Records a ros_control ControllerState data to CSV for Matlab/etc analysis 37 | */ 38 | 39 | #ifndef GENERIC_ROS_CONTROL__CSV_TO_CONTROLLER_H 40 | #define GENERIC_ROS_CONTROL__CSV_TO_CONTROLLER_H 41 | 42 | // ROS 43 | #include 44 | 45 | // ros_control 46 | #include 47 | #include 48 | 49 | // actionlib 50 | #include 51 | #include 52 | 53 | namespace ros_control_boilerplate 54 | { 55 | static const double RECORD_RATE_HZ = 100.0; // times per second to record 56 | 57 | class CSVToController 58 | { 59 | public: 60 | /** 61 | * \brief Constructor 62 | */ 63 | CSVToController(const std::string& joint_trajectory_action, const std::string& controller_state_topic); 64 | 65 | /** \brief Callback from ROS message */ 66 | void stateCB(const control_msgs::JointTrajectoryControllerState::ConstPtr& state); 67 | 68 | void printPoint(trajectory_msgs::JointTrajectoryPoint& point); 69 | 70 | // Start the data collection 71 | void loadAndRunCSV(const std::string& file_name); 72 | 73 | private: 74 | // A shared node handle 75 | ros::NodeHandle nh_; 76 | 77 | // Listener to state of controller 78 | ros::Subscriber state_sub_; 79 | 80 | // Action 81 | actionlib::SimpleActionClient joint_trajectory_action_; 82 | 83 | // Where to save the CSV 84 | std::string file_name_; 85 | std::string controller_state_topic_; 86 | 87 | // Cache of last recieved state 88 | control_msgs::JointTrajectoryControllerState current_state_; 89 | 90 | }; // end class 91 | 92 | } // namespace ros_control_boilerplate 93 | 94 | #endif 95 | -------------------------------------------------------------------------------- /include/ros_control_boilerplate/tools/joystick_manual_control.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Inherit from this file to enable joystick mode switching of your robot 37 | */ 38 | 39 | #ifndef ROS_CONTROL_BOILERPLATE__JOYSTICK_MANUAL_CONTROL 40 | #define ROS_CONTROL_BOILERPLATE__JOYSTICK_MANUAL_CONTROL 41 | 42 | // ROS 43 | #include 44 | #include 45 | 46 | // ros_control 47 | #include 48 | #include 49 | 50 | namespace ros_control_boilerplate 51 | { 52 | class JoystickManualControl 53 | { 54 | public: 55 | /** 56 | * \brief Constructor 57 | * \param parent_name - name of parent class, only used for namespacing logging debug output 58 | * \param service_namespace - prefix to controller manager service, or blank. do not add trailing 59 | * "/" 60 | */ 61 | JoystickManualControl(const std::string& parent_name, const std::string& service_namespace) 62 | : parent_name_(parent_name), using_trajectory_controller_(true) 63 | { 64 | switch_service_ = service_namespace + "/controller_manager/switch_controller"; 65 | load_service_ = service_namespace + "/controller_manager/load_controller"; 66 | 67 | // Switch modes of controllers 68 | switch_controlers_client_ = nh_.serviceClient(switch_service_); 69 | load_controlers_client_ = nh_.serviceClient(load_service_); 70 | 71 | // Subscribe to joystick control 72 | std::size_t queue_size = 1; 73 | remote_joy_ = nh_.subscribe("/joy", queue_size, &JoystickManualControl::joyCallback, this); 74 | 75 | ROS_INFO_STREAM_NAMED(parent_name_, "JoystickManualControl Ready."); 76 | } 77 | 78 | /** 79 | * \brief Response to joystick control 80 | * Button mapping is customized by each class that inherits from this 81 | */ 82 | virtual void joyCallback(const sensor_msgs::Joy::ConstPtr& msg) = 0; 83 | 84 | /** 85 | * \brief Load a secondary manual controller 86 | */ 87 | bool loadManualControllers() 88 | { 89 | // Ensure services are up 90 | ROS_INFO_STREAM_NAMED(parent_name_, "Waiting for serivces..."); 91 | if (!ros::service::waitForService(switch_service_, ros::Duration(10))) 92 | ROS_ERROR_STREAM_NAMED(parent_name_, "Unable to find service " << switch_service_); 93 | if (!ros::service::waitForService(load_service_, ros::Duration(10))) 94 | ROS_ERROR_STREAM_NAMED(parent_name_, "Unable to find service " << load_service_); 95 | 96 | for (std::size_t i = 0; i < manual_controllers_.size(); ++i) 97 | { 98 | ROS_INFO_STREAM_NAMED(parent_name_, "Loading controller " << manual_controllers_[i]); 99 | controller_manager_msgs::LoadController service; 100 | service.request.name = manual_controllers_[i]; 101 | std::size_t counter = 0; 102 | while (!load_controlers_client_.call(service) && ros::ok()) 103 | { 104 | if (counter > 100) 105 | ROS_WARN_STREAM_THROTTLE_NAMED(1.0, parent_name_, 106 | "Failed to load controller '" << manual_controllers_[i] << "', trying again"); 107 | ros::spinOnce(); 108 | ros::Duration(0.1).sleep(); 109 | counter++; 110 | } 111 | } 112 | 113 | return true; 114 | } 115 | 116 | void switchToManual() 117 | { 118 | // Stop all controllers, soft E-Stop 119 | controller_manager_msgs::SwitchController service; 120 | service.request.strictness = service.request.STRICT; 121 | 122 | ROS_WARN_STREAM_NAMED(parent_name_, "Switching to MANUAL control"); 123 | for (std::size_t i = 0; i < manual_controllers_.size(); ++i) 124 | { 125 | service.request.start_controllers.push_back(manual_controllers_[i]); 126 | } 127 | for (std::size_t i = 0; i < trajectory_controllers_.size(); ++i) 128 | { 129 | service.request.stop_controllers.push_back(trajectory_controllers_[i]); 130 | } 131 | 132 | // Attempt stop 133 | if (!switch_controlers_client_.call(service)) 134 | { 135 | ROS_ERROR_STREAM_NAMED(parent_name_, "Failed to switch controllers"); 136 | return; 137 | } 138 | } 139 | 140 | void switchToTrajectory() 141 | { 142 | // Stop all controllers, soft E-Stop 143 | controller_manager_msgs::SwitchController service; 144 | service.request.strictness = service.request.STRICT; 145 | 146 | ROS_INFO_STREAM_NAMED(parent_name_, "Switching to TRAJECTORY control"); 147 | for (std::size_t i = 0; i < manual_controllers_.size(); ++i) 148 | { 149 | service.request.stop_controllers.push_back(manual_controllers_[i]); 150 | } 151 | for (std::size_t i = 0; i < trajectory_controllers_.size(); ++i) 152 | { 153 | service.request.start_controllers.push_back(trajectory_controllers_[i]); 154 | } 155 | 156 | // Attempt stop 157 | if (!switch_controlers_client_.call(service)) 158 | { 159 | ROS_ERROR_STREAM_NAMED(parent_name_, "Failed to switch controllers"); 160 | return; 161 | } 162 | } 163 | 164 | protected: 165 | // A shared node handle 166 | ros::NodeHandle nh_; 167 | 168 | // Name of parent class, used for logging messages 169 | const std::string parent_name_; 170 | std::string switch_service_; 171 | std::string load_service_; 172 | 173 | // Subscribe to joystick commands 174 | ros::Subscriber remote_joy_; 175 | 176 | // Ability to switch controllers 177 | ros::ServiceClient switch_controlers_client_; 178 | ros::ServiceClient load_controlers_client_; 179 | 180 | // Switching controller mode 181 | bool using_trajectory_controller_; 182 | 183 | // Controller lists 184 | std::vector manual_controllers_; 185 | std::vector trajectory_controllers_; 186 | 187 | }; // end class 188 | 189 | } // namespace ros_control_boilerplate 190 | 191 | #endif 192 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ros_control_boilerplate 4 | 0.6.1 5 | Simple simulation interface and template for setting up a hardware interface for ros_control 6 | 7 | Dave Coleman 8 | 9 | BSD 10 | 11 | https://github.com/davetcoleman/ros_control_boilerplate 12 | https://github.com/davetcoleman/ros_control_boilerplate/issues 13 | https://github.com/davetcoleman/ros_control_boilerplate/ 14 | 15 | Dave Coleman 16 | 17 | catkin 18 | 19 | hardware_interface 20 | controller_manager 21 | roscpp 22 | control_msgs 23 | trajectory_msgs 24 | actionlib 25 | urdf 26 | joint_limits_interface 27 | transmission_interface 28 | control_toolbox 29 | std_msgs 30 | cmake_modules 31 | libgflags-dev 32 | sensor_msgs 33 | rosparam_shortcuts 34 | 35 | hardware_interface 36 | controller_manager 37 | roscpp 38 | control_msgs 39 | trajectory_msgs 40 | actionlib 41 | urdf 42 | joint_limits_interface 43 | transmission_interface 44 | control_toolbox 45 | std_msgs 46 | sensor_msgs 47 | rosparam_shortcuts 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /resources/screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/ros_control_boilerplate/0f9cd01bc1430c718554a3801683ada9cd2aa0a6/resources/screenshot.png -------------------------------------------------------------------------------- /rrbot_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include_directories( 2 | include/ 3 | ) 4 | 5 | # RRBot Hardware Interface 6 | add_library(rrbot_hw_interface 7 | src/rrbot_hw_interface.cpp 8 | ) 9 | target_link_libraries(rrbot_hw_interface 10 | generic_hw_interface 11 | ${catkin_LIBRARIES} 12 | ) 13 | 14 | # Main control executable 15 | add_executable(rrbot_hw_main src/rrbot_hw_main.cpp) 16 | target_link_libraries(rrbot_hw_main 17 | rrbot_hw_interface 18 | generic_hw_control_loop 19 | ${catkin_LIBRARIES} 20 | ) 21 | 22 | ## Install ------------------------------------------------------------ 23 | 24 | # Install libraries 25 | install(TARGETS 26 | rrbot_hw_interface 27 | LIBRARY DESTINATION 28 | ${CATKIN_PACKAGE_LIB_DESTINATION} 29 | ) 30 | 31 | # Install executables 32 | install(TARGETS 33 | rrbot_hw_main 34 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 35 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 36 | ) 37 | 38 | # Install header files 39 | install(DIRECTORY include/rrbot_control/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 40 | -------------------------------------------------------------------------------- /rrbot_control/config/rrbot_controllers.yaml: -------------------------------------------------------------------------------- 1 | # ros_control_boilerplate Settings ----------------------- 2 | # Settings for ros_control control loop 3 | generic_hw_control_loop: 4 | loop_hz: 300 5 | cycle_time_error_threshold: 0.01 6 | 7 | # Settings for ros_control hardware interface 8 | hardware_interface: 9 | joints: 10 | - joint1 11 | - joint2 12 | sim_control_mode: 0 # 0: position, 1: velocity 13 | 14 | # Publish all joint states ---------------------------------- 15 | # Creates the /joint_states topic necessary in ROS 16 | joint_state_controller: 17 | type: joint_state_controller/JointStateController 18 | publish_rate: 50 19 | 20 | # Joint Trajectory Controller ------------------------------- 21 | # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller 22 | position_trajectory_controller: 23 | type: position_controllers/JointTrajectoryController 24 | # These joints can likely just be copied from the hardware_interface list above 25 | joints: 26 | - joint1 27 | - joint2 28 | constraints: 29 | goal_time: 5.0 30 | #stopped_position_tolerance: 0.4 # Defaults to 0.01 31 | joint1: 32 | trajectory: 0.60 33 | goal: 0.15 34 | joint2: 35 | trajectory: 0.60 36 | goal: 0.15 37 | # gains: 38 | # joint1: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} 39 | # joint2: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} 40 | 41 | # state_publish_rate: 50 # Defaults to 50 42 | # action_monitor_rate: 20 # Defaults to 20 43 | #hold_trajectory_duration: 0 # Defaults to 0.5 44 | 45 | # Individual Position Controllers --------------------------------------- 46 | # Allows to send individual ROS msg of Float64 to each joint separately 47 | joint1_position_controller: 48 | type: position_controllers/JointPositionController 49 | joint: joint1 50 | pid: {p: 100.0, i: 0.01, d: 10.0} 51 | joint2_position_controller: 52 | type: position_controllers/JointPositionController 53 | joint: joint2 54 | pid: {p: 100.0, i: 0.01, d: 10.0} 55 | 56 | # Group Position Controllers --------------------------------------- 57 | # Allows to send single ROS msg of Float64MultiArray to all joints 58 | joint_position_controller: 59 | type: position_controllers/JointGroupPositionController 60 | joints: 61 | - joint1 62 | - joint2 63 | -------------------------------------------------------------------------------- /rrbot_control/include/rrbot_control/rrbot_hw_interface.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Example ros_control hardware interface blank template for the RRBot 37 | For a more detailed simulation example, see sim_hw_interface.h 38 | */ 39 | 40 | #ifndef RRBOT_CONTROL__RRBOT_HW_INTERFACE_H 41 | #define RRBOT_CONTROL__RRBOT_HW_INTERFACE_H 42 | 43 | #include 44 | 45 | namespace rrbot_control 46 | { 47 | /// \brief Hardware interface for a robot 48 | class RRBotHWInterface : public ros_control_boilerplate::GenericHWInterface 49 | { 50 | public: 51 | /** 52 | * \brief Constructor 53 | * \param nh - Node handle for topics. 54 | */ 55 | RRBotHWInterface(ros::NodeHandle& nh, urdf::Model* urdf_model = NULL); 56 | 57 | /** \brief Read the state from the robot hardware. */ 58 | virtual void read(ros::Duration& elapsed_time); 59 | 60 | /** \brief Write the command to the robot hardware. */ 61 | virtual void write(ros::Duration& elapsed_time); 62 | 63 | /** \brief Enforce limits for all values before writing */ 64 | virtual void enforceLimits(ros::Duration& period); 65 | 66 | }; // class 67 | 68 | } // namespace rrbot_control 69 | 70 | #endif 71 | -------------------------------------------------------------------------------- /rrbot_control/launch/rrbot_hardware.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 17 | 18 | 19 | 20 | 21 | 22 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /rrbot_control/launch/rrbot_simulation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 17 | 18 | 19 | 20 | 21 | 22 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /rrbot_control/launch/rrbot_test_trajectory.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /rrbot_control/launch/rrbot_visualize.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /rrbot_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rrbot_control 4 | 0.0.1 5 | Simple simulation interface and template for setting up a hardware interface for rrbot 6 | 7 | Someone 8 | 9 | BSD 10 | 11 | https://github.com/davetcoleman/ros_control_boilerplate 12 | https://github.com/davetcoleman/ros_control_boilerplate/issues 13 | https://github.com/davetcoleman/ros_control_boilerplate/ 14 | 15 | Dave Coleman 16 | 17 | catkin 18 | 19 | ros_control_boilerplate 20 | 21 | ros_control_boilerplate 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /rrbot_control/src/rrbot_hw_interface.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Example ros_control hardware interface blank template for the RRBot 37 | For a more detailed simulation example, see sim_hw_interface.cpp 38 | */ 39 | 40 | #include 41 | 42 | namespace rrbot_control 43 | { 44 | RRBotHWInterface::RRBotHWInterface(ros::NodeHandle& nh, urdf::Model* urdf_model) 45 | : ros_control_boilerplate::GenericHWInterface(nh, urdf_model) 46 | { 47 | ROS_INFO_NAMED("rrbot_hw_interface", "RRBotHWInterface Ready."); 48 | } 49 | 50 | void RRBotHWInterface::read(ros::Duration& elapsed_time) 51 | { 52 | // ---------------------------------------------------- 53 | // ---------------------------------------------------- 54 | // ---------------------------------------------------- 55 | // 56 | // FILL IN YOUR READ COMMAND FROM USB/ETHERNET/ETHERCAT/SERIAL ETC HERE 57 | // 58 | // ---------------------------------------------------- 59 | // ---------------------------------------------------- 60 | // ---------------------------------------------------- 61 | } 62 | 63 | void RRBotHWInterface::write(ros::Duration& elapsed_time) 64 | { 65 | // Safety 66 | enforceLimits(elapsed_time); 67 | 68 | // ---------------------------------------------------- 69 | // ---------------------------------------------------- 70 | // ---------------------------------------------------- 71 | // 72 | // FILL IN YOUR WRITE COMMAND TO USB/ETHERNET/ETHERCAT/SERIAL ETC HERE 73 | // 74 | // FOR A EASY SIMULATION EXAMPLE, OR FOR CODE TO CALCULATE 75 | // VELOCITY FROM POSITION WITH SMOOTHING, SEE 76 | // sim_hw_interface.cpp IN THIS PACKAGE 77 | // 78 | // DUMMY PASS-THROUGH CODE 79 | for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id) 80 | joint_position_[joint_id] += joint_position_command_[joint_id]; 81 | // END DUMMY CODE 82 | // 83 | // ---------------------------------------------------- 84 | // ---------------------------------------------------- 85 | // ---------------------------------------------------- 86 | } 87 | 88 | void RRBotHWInterface::enforceLimits(ros::Duration& period) 89 | { 90 | // ---------------------------------------------------- 91 | // ---------------------------------------------------- 92 | // ---------------------------------------------------- 93 | // 94 | // CHOOSE THE TYPE OF JOINT LIMITS INTERFACE YOU WANT TO USE 95 | // YOU SHOULD ONLY NEED TO USE ONE SATURATION INTERFACE, 96 | // DEPENDING ON YOUR CONTROL METHOD 97 | // 98 | // EXAMPLES: 99 | // 100 | // Saturation Limits --------------------------- 101 | // 102 | // Enforces position and velocity 103 | pos_jnt_sat_interface_.enforceLimits(period); 104 | // 105 | // Enforces velocity and acceleration limits 106 | // vel_jnt_sat_interface_.enforceLimits(period); 107 | // 108 | // Enforces position, velocity, and effort 109 | // eff_jnt_sat_interface_.enforceLimits(period); 110 | 111 | // Soft limits --------------------------------- 112 | // 113 | // pos_jnt_soft_limits_.enforceLimits(period); 114 | // vel_jnt_soft_limits_.enforceLimits(period); 115 | // eff_jnt_soft_limits_.enforceLimits(period); 116 | // 117 | // ---------------------------------------------------- 118 | // ---------------------------------------------------- 119 | // ---------------------------------------------------- 120 | } 121 | 122 | } // namespace rrbot_control 123 | -------------------------------------------------------------------------------- /rrbot_control/src/rrbot_hw_main.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Example ros_control main() entry point for controlling robots in ROS 37 | */ 38 | 39 | #include 40 | #include 41 | 42 | int main(int argc, char** argv) 43 | { 44 | ros::init(argc, argv, "rrbot_hw_interface"); 45 | ros::NodeHandle nh; 46 | 47 | // NOTE: We run the ROS loop in a separate thread as external calls such 48 | // as service callbacks to load controllers can block the (main) control loop 49 | ros::AsyncSpinner spinner(3); 50 | spinner.start(); 51 | 52 | // Create the hardware interface specific to your robot 53 | std::shared_ptr rrbot_hw_interface(new rrbot_control::RRBotHWInterface(nh)); 54 | rrbot_hw_interface->init(); 55 | 56 | // Start the control loop 57 | ros_control_boilerplate::GenericHWControlLoop control_loop(nh, rrbot_hw_interface); 58 | control_loop.run(); // Blocks until shutdown signal recieved 59 | 60 | return 0; 61 | } 62 | -------------------------------------------------------------------------------- /scripts/analyze_trajectory.m: -------------------------------------------------------------------------------- 1 | clear 2 | clc 3 | format long g 4 | clf 5 | 6 | while 1 7 | sampled_data = csvread('~/ros/current/ws_acme/src/iiwa_trajectory_data/recorded_trajectory_2.csv',1,0); 8 | %assign columns into seperate variable names - this is made quickly using the 9 | %header names and find-replace command 10 | i=1; 11 | timestamp=sampled_data(:,i);i=i+1; 12 | t_delta=diff(timestamp); 13 | 14 | j1_pos_desired=sampled_data(:,i);i=i+1; 15 | j1_vel_desired=sampled_data(:,i);i=i+1; 16 | j1_pos=sampled_data(:,i);i=i+1; 17 | j1_vel=sampled_data(:,i);i=i+1; 18 | j1_vel_delta=diff(j1_vel); 19 | j1_accel=[0; j1_vel_delta ./ t_delta]; % add a zero in front to make same size as other data structures 20 | 21 | j2_pos_desired=sampled_data(:,i);i=i+1; 22 | j2_vel_desired=sampled_data(:,i);i=i+1; 23 | j2_pos=sampled_data(:,i);i=i+1; 24 | j2_vel=sampled_data(:,i);i=i+1; 25 | j2_vel_delta=diff(j2_vel); 26 | j2_accel=[0; j2_vel_delta ./ t_delta]; % add a zero in front to make same size as other data structures 27 | 28 | j3_pos_desired=sampled_data(:,i);i=i+1; 29 | j3_vel_desired=sampled_data(:,i);i=i+1; 30 | j3_pos=sampled_data(:,i);i=i+1; 31 | j3_vel=sampled_data(:,i);i=i+1; 32 | j3_vel_delta=diff(j3_vel); 33 | j3_accel=[0; j3_vel_delta ./ t_delta]; % add a zero in front to make same size as other data structures 34 | 35 | j4_pos_desired=sampled_data(:,i);i=i+1; 36 | j4_vel_desired=sampled_data(:,i);i=i+1; 37 | j4_pos=sampled_data(:,i);i=i+1; 38 | j4_vel=sampled_data(:,i);i=i+1; 39 | j4_vel_delta=diff(j4_vel); 40 | j4_accel=[0; j4_vel_delta ./ t_delta]; % add a zero in front to make same size as other data structures 41 | 42 | j5_pos_desired=sampled_data(:,i);i=i+1; 43 | j5_vel_desired=sampled_data(:,i);i=i+1; 44 | j5_pos=sampled_data(:,i);i=i+1; 45 | j5_vel=sampled_data(:,i);i=i+1; 46 | j5_vel_delta=diff(j5_vel); 47 | j5_accel=[0; j5_vel_delta ./ t_delta]; % add a zero in front to make same size as other data structures 48 | 49 | j6_pos_desired=sampled_data(:,i);i=i+1; 50 | j6_vel_desired=sampled_data(:,i);i=i+1; 51 | j6_pos=sampled_data(:,i);i=i+1; 52 | j6_vel=sampled_data(:,i);i=i+1; 53 | j6_vel_delta=diff(j6_vel); 54 | j6_accel=[0; j6_vel_delta ./ t_delta]; % add a zero in front to make same size as other data structures 55 | 56 | j7_pos_desired=sampled_data(:,i);i=i+1; 57 | j7_vel_desired=sampled_data(:,i);i=i+1; 58 | j7_pos=sampled_data(:,i);i=i+1; 59 | j7_vel=sampled_data(:,i);i=i+1; 60 | j7_vel_delta=diff(j7_vel); 61 | j7_accel=[0; j7_vel_delta ./ t_delta]; % add a zero in front to make same size as other data structures 62 | 63 | time_start_end = [timestamp(1,1) timestamp(end,1)]; 64 | 65 | if (0) %plot position actual 66 | plot(... 67 | timestamp, j1_pos,'r', ... 68 | timestamp, j2_pos,'y', ... 69 | timestamp, j3_pos,'m', ... 70 | timestamp, j4_pos,'c',... 71 | timestamp, j5_pos,'g',... 72 | timestamp, j6_pos,'b',... 73 | timestamp, j7_pos,'k') 74 | 75 | xlabel('Time') 76 | ylabel('Position') 77 | legend('Joint 1 Pos',... 78 | 'Joint 2 Pos',... 79 | 'Joint 3 Pos',... 80 | 'Joint 4 Pos',... 81 | 'Joint 5 Pos',... 82 | 'Joint 6 Pos',... 83 | 'Joint 7 Pos') 84 | 85 | title('Position Trajectory') 86 | 87 | elseif(0) %plot velocity actual 88 | plot(... 89 | timestamp, j1_vel,'r',... 90 | timestamp, j2_vel,'y',... 91 | timestamp, j3_vel,'m',... 92 | timestamp, j4_vel,'c',... 93 | timestamp, j5_vel,'g',... 94 | timestamp, j6_vel,'b',... 95 | timestamp, j7_vel,'k') 96 | 97 | xlabel('Time') 98 | ylabel('Velocity') 99 | legend('Joint 1 vel',... 100 | 'Joint 2 vel',... 101 | 'Joint 3 vel',... 102 | 'Joint 4 vel',... 103 | 'Joint 5 vel',... 104 | 'Joint 6 vel',... 105 | 'Joint 7 vel') 106 | 107 | title('Velocity Actual Trajectory') 108 | elseif(0) %plot velocity desired with actual 109 | plot(... 110 | timestamp, j1_vel_desired,'--r',... 111 | timestamp, j2_vel_desired,'--y',... 112 | timestamp, j3_vel_desired,'--m',... 113 | timestamp, j4_vel_desired,'--c',... 114 | timestamp, j5_vel_desired,'--g',... 115 | timestamp, j6_vel_desired,'--b',... 116 | timestamp, j7_vel_desired,'--k',... 117 | timestamp, j1_vel,'r',... 118 | timestamp, j2_vel,'y',... 119 | timestamp, j3_vel,'m',... 120 | timestamp, j4_vel,'c',... 121 | timestamp, j5_vel,'g',... 122 | timestamp, j6_vel,'b',... 123 | timestamp, j7_vel,'k') 124 | 125 | xlabel('Time') 126 | ylabel('Velocity') 127 | legend('Joint 1',... 128 | 'Joint 2',... 129 | 'Joint 3',... 130 | 'Joint 4',... 131 | 'Joint 5',... 132 | 'Joint 6',... 133 | 'Joint 7') 134 | 135 | title('Velocity DESIRED Trajectory') 136 | elseif(1) %plot acceleration desired with actual 137 | plot(... 138 | timestamp, j1_accel,'--r',... 139 | timestamp, j2_accel,'--y',... 140 | timestamp, j3_accel,'--m',... 141 | timestamp, j4_accel,'--c',... 142 | timestamp, j5_accel,'--g',... 143 | timestamp, j6_accel,'--b',... 144 | timestamp, j7_accel,'--k') %,... 145 | % timestamp, j1_vel,'r',... 146 | % timestamp, j2_vel,'y',... 147 | % timestamp, j3_vel,'m',... 148 | % timestamp, j4_vel,'c',... 149 | % timestamp, j5_vel,'g',... 150 | % timestamp, j6_vel,'b',... 151 | % timestamp, j7_vel,'k') 152 | 153 | xlabel('Time') 154 | ylabel('acceleration') 155 | legend('Joint 1',... 156 | 'Joint 2',... 157 | 'Joint 3',... 158 | 'Joint 4',... 159 | 'Joint 5',... 160 | 'Joint 6',... 161 | 'Joint 7') 162 | 163 | title('Acceleration') 164 | 165 | end 166 | 167 | %pause(1) 168 | break 169 | end -------------------------------------------------------------------------------- /src/generic_hw_control_loop.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Example control loop for reading, updating, and writing commands to a 37 | hardware interface using MONOTOIC system time 38 | */ 39 | 40 | #include 41 | 42 | // ROS parameter loading 43 | #include 44 | 45 | namespace ros_control_boilerplate 46 | { 47 | GenericHWControlLoop::GenericHWControlLoop(ros::NodeHandle& nh, 48 | std::shared_ptr hardware_interface) 49 | : nh_(nh), hardware_interface_(hardware_interface) 50 | { 51 | // Create the controller manager 52 | controller_manager_.reset(new controller_manager::ControllerManager(hardware_interface_.get(), nh_)); 53 | 54 | // Load rosparams 55 | ros::NodeHandle rpsnh(nh, name_); 56 | std::size_t error = 0; 57 | error += !rosparam_shortcuts::get(name_, rpsnh, "loop_hz", loop_hz_); 58 | error += !rosparam_shortcuts::get(name_, rpsnh, "cycle_time_error_threshold", cycle_time_error_threshold_); 59 | rosparam_shortcuts::shutdownIfError(name_, error); 60 | 61 | // Get current time for use with first update 62 | clock_gettime(CLOCK_MONOTONIC, &last_time_); 63 | 64 | desired_update_period_ = ros::Duration(1 / loop_hz_); 65 | } 66 | 67 | void GenericHWControlLoop::run() 68 | { 69 | ros::Rate rate(loop_hz_); 70 | while (ros::ok()) 71 | { 72 | update(); 73 | rate.sleep(); 74 | } 75 | } 76 | 77 | void GenericHWControlLoop::update() 78 | { 79 | // Get change in time 80 | clock_gettime(CLOCK_MONOTONIC, ¤t_time_); 81 | elapsed_time_ = 82 | ros::Duration(current_time_.tv_sec - last_time_.tv_sec + (current_time_.tv_nsec - last_time_.tv_nsec) / BILLION); 83 | last_time_ = current_time_; 84 | ros::Time now = ros::Time::now(); 85 | // ROS_DEBUG_STREAM_THROTTLE_NAMED(1, "generic_hw_main","Sampled update loop 86 | // with elapsed time " << elapsed_time_.toSec()); 87 | 88 | // Error check cycle time 89 | const double cycle_time_error = (elapsed_time_ - desired_update_period_).toSec(); 90 | if (cycle_time_error > cycle_time_error_threshold_) 91 | { 92 | ROS_WARN_STREAM_NAMED(name_, "Cycle time exceeded error threshold by: " 93 | << cycle_time_error << ", cycle time: " << elapsed_time_ 94 | << ", threshold: " << cycle_time_error_threshold_); 95 | } 96 | 97 | // Input 98 | hardware_interface_->read(now, elapsed_time_); 99 | 100 | // Control 101 | controller_manager_->update(now, elapsed_time_); 102 | 103 | // Output 104 | hardware_interface_->write(now, elapsed_time_); 105 | } 106 | 107 | } // namespace ros_control_boilerplate 108 | -------------------------------------------------------------------------------- /src/generic_hw_interface.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, PickNik LLC 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of PickNik LLC nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Helper ros_control hardware interface that loads configurations 37 | */ 38 | 39 | #include 40 | #include 41 | 42 | // ROS parameter loading 43 | #include 44 | 45 | namespace ros_control_boilerplate 46 | { 47 | GenericHWInterface::GenericHWInterface(const ros::NodeHandle& nh, urdf::Model* urdf_model) 48 | : name_("generic_hw_interface"), nh_(nh), use_rosparam_joint_limits_(false), use_soft_limits_if_available_(false) 49 | { 50 | // Check if the URDF model needs to be loaded 51 | if (urdf_model == NULL) 52 | loadURDF(nh, "robot_description"); 53 | else 54 | urdf_model_ = urdf_model; 55 | 56 | // Load rosparams 57 | ros::NodeHandle rpnh( 58 | nh_, "hardware_interface"); // TODO(davetcoleman): change the namespace to "generic_hw_interface" aka name_ 59 | std::size_t error = 0; 60 | error += !rosparam_shortcuts::get(name_, rpnh, "joints", joint_names_); 61 | rosparam_shortcuts::shutdownIfError(name_, error); 62 | } 63 | 64 | void GenericHWInterface::init() 65 | { 66 | num_joints_ = joint_names_.size(); 67 | 68 | // Status 69 | joint_position_.resize(num_joints_, 0.0); 70 | joint_velocity_.resize(num_joints_, 0.0); 71 | joint_effort_.resize(num_joints_, 0.0); 72 | 73 | // Command 74 | joint_position_command_.resize(num_joints_, 0.0); 75 | joint_velocity_command_.resize(num_joints_, 0.0); 76 | joint_effort_command_.resize(num_joints_, 0.0); 77 | 78 | // Limits 79 | joint_position_lower_limits_.resize(num_joints_, 0.0); 80 | joint_position_upper_limits_.resize(num_joints_, 0.0); 81 | joint_velocity_limits_.resize(num_joints_, 0.0); 82 | joint_effort_limits_.resize(num_joints_, 0.0); 83 | 84 | // Initialize interfaces for each joint 85 | for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id) 86 | { 87 | ROS_DEBUG_STREAM_NAMED(name_, "Loading joint name: " << joint_names_[joint_id]); 88 | 89 | // Create joint state interface 90 | joint_state_interface_.registerHandle(hardware_interface::JointStateHandle( 91 | joint_names_[joint_id], &joint_position_[joint_id], &joint_velocity_[joint_id], &joint_effort_[joint_id])); 92 | 93 | // Add command interfaces to joints 94 | // TODO: decide based on transmissions? 95 | hardware_interface::JointHandle joint_handle_position = hardware_interface::JointHandle( 96 | joint_state_interface_.getHandle(joint_names_[joint_id]), &joint_position_command_[joint_id]); 97 | position_joint_interface_.registerHandle(joint_handle_position); 98 | 99 | hardware_interface::JointHandle joint_handle_velocity = hardware_interface::JointHandle( 100 | joint_state_interface_.getHandle(joint_names_[joint_id]), &joint_velocity_command_[joint_id]); 101 | velocity_joint_interface_.registerHandle(joint_handle_velocity); 102 | 103 | hardware_interface::JointHandle joint_handle_effort = hardware_interface::JointHandle( 104 | joint_state_interface_.getHandle(joint_names_[joint_id]), &joint_effort_command_[joint_id]); 105 | effort_joint_interface_.registerHandle(joint_handle_effort); 106 | 107 | // Load the joint limits 108 | registerJointLimits(joint_handle_position, joint_handle_velocity, joint_handle_effort, joint_id); 109 | } // end for each joint 110 | 111 | registerInterface(&joint_state_interface_); // From RobotHW base class. 112 | registerInterface(&position_joint_interface_); // From RobotHW base class. 113 | registerInterface(&velocity_joint_interface_); // From RobotHW base class. 114 | registerInterface(&effort_joint_interface_); // From RobotHW base class. 115 | 116 | ROS_INFO_STREAM_NAMED(name_, "GenericHWInterface Ready."); 117 | } 118 | 119 | void GenericHWInterface::registerJointLimits(const hardware_interface::JointHandle& joint_handle_position, 120 | const hardware_interface::JointHandle& joint_handle_velocity, 121 | const hardware_interface::JointHandle& joint_handle_effort, 122 | std::size_t joint_id) 123 | { 124 | // Default values 125 | joint_position_lower_limits_[joint_id] = -std::numeric_limits::max(); 126 | joint_position_upper_limits_[joint_id] = std::numeric_limits::max(); 127 | joint_velocity_limits_[joint_id] = std::numeric_limits::max(); 128 | joint_effort_limits_[joint_id] = std::numeric_limits::max(); 129 | 130 | // Limits datastructures 131 | joint_limits_interface::JointLimits joint_limits; // Position 132 | joint_limits_interface::SoftJointLimits soft_limits; // Soft Position 133 | bool has_joint_limits = false; 134 | bool has_soft_limits = false; 135 | 136 | // Get limits from URDF 137 | if (urdf_model_ == NULL) 138 | { 139 | ROS_WARN_STREAM_NAMED(name_, "No URDF model loaded, unable to get joint limits"); 140 | return; 141 | } 142 | 143 | // Get limits from URDF 144 | urdf::JointConstSharedPtr urdf_joint = urdf_model_->getJoint(joint_names_[joint_id]); 145 | 146 | // Get main joint limits 147 | if (urdf_joint == NULL) 148 | { 149 | ROS_ERROR_STREAM_NAMED(name_, "URDF joint not found " << joint_names_[joint_id]); 150 | return; 151 | } 152 | 153 | // Get limits from URDF 154 | if (joint_limits_interface::getJointLimits(urdf_joint, joint_limits)) 155 | { 156 | has_joint_limits = true; 157 | ROS_DEBUG_STREAM_NAMED(name_, "Joint " << joint_names_[joint_id] << " has URDF position limits [" 158 | << joint_limits.min_position << ", " << joint_limits.max_position << "]"); 159 | if (joint_limits.has_velocity_limits) 160 | ROS_DEBUG_STREAM_NAMED(name_, "Joint " << joint_names_[joint_id] << " has URDF velocity limit [" 161 | << joint_limits.max_velocity << "]"); 162 | } 163 | else 164 | { 165 | if (urdf_joint->type != urdf::Joint::CONTINUOUS) 166 | ROS_WARN_STREAM_NAMED(name_, "Joint " << joint_names_[joint_id] 167 | << " does not have a URDF " 168 | "position limit"); 169 | } 170 | 171 | // Get limits from ROS param 172 | if (use_rosparam_joint_limits_) 173 | { 174 | if (joint_limits_interface::getJointLimits(joint_names_[joint_id], nh_, joint_limits)) 175 | { 176 | has_joint_limits = true; 177 | ROS_DEBUG_STREAM_NAMED(name_, "Joint " << joint_names_[joint_id] << " has rosparam position limits [" 178 | << joint_limits.min_position << ", " << joint_limits.max_position << "]"); 179 | if (joint_limits.has_velocity_limits) 180 | ROS_DEBUG_STREAM_NAMED(name_, "Joint " << joint_names_[joint_id] << " has rosparam velocity limit [" 181 | << joint_limits.max_velocity << "]"); 182 | } // the else debug message provided internally by joint_limits_interface 183 | } 184 | 185 | // Get soft limits from URDF 186 | if (use_soft_limits_if_available_) 187 | { 188 | if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits)) 189 | { 190 | has_soft_limits = true; 191 | ROS_DEBUG_STREAM_NAMED(name_, "Joint " << joint_names_[joint_id] << " has soft joint limits."); 192 | } 193 | else 194 | { 195 | ROS_DEBUG_STREAM_NAMED(name_, "Joint " << joint_names_[joint_id] 196 | << " does not have soft joint " 197 | "limits"); 198 | } 199 | } 200 | 201 | // Quit we we haven't found any limits in URDF or rosparam server 202 | if (!has_joint_limits) 203 | { 204 | return; 205 | } 206 | 207 | // Copy position limits if available 208 | if (joint_limits.has_position_limits) 209 | { 210 | // Slighly reduce the joint limits to prevent floating point errors 211 | joint_limits.min_position += std::numeric_limits::epsilon(); 212 | joint_limits.max_position -= std::numeric_limits::epsilon(); 213 | 214 | joint_position_lower_limits_[joint_id] = joint_limits.min_position; 215 | joint_position_upper_limits_[joint_id] = joint_limits.max_position; 216 | } 217 | 218 | // Copy velocity limits if available 219 | if (joint_limits.has_velocity_limits) 220 | { 221 | joint_velocity_limits_[joint_id] = joint_limits.max_velocity; 222 | } 223 | 224 | // Copy effort limits if available 225 | if (joint_limits.has_effort_limits) 226 | { 227 | joint_effort_limits_[joint_id] = joint_limits.max_effort; 228 | } 229 | 230 | if (has_soft_limits) // Use soft limits 231 | { 232 | ROS_DEBUG_STREAM_NAMED(name_, "Using soft saturation limits"); 233 | const joint_limits_interface::PositionJointSoftLimitsHandle soft_handle_position(joint_handle_position, 234 | joint_limits, soft_limits); 235 | pos_jnt_soft_limits_.registerHandle(soft_handle_position); 236 | const joint_limits_interface::VelocityJointSoftLimitsHandle soft_handle_velocity(joint_handle_velocity, 237 | joint_limits, soft_limits); 238 | vel_jnt_soft_limits_.registerHandle(soft_handle_velocity); 239 | const joint_limits_interface::EffortJointSoftLimitsHandle soft_handle_effort(joint_handle_effort, joint_limits, 240 | soft_limits); 241 | eff_jnt_soft_limits_.registerHandle(soft_handle_effort); 242 | } 243 | else // Use saturation limits 244 | { 245 | ROS_DEBUG_STREAM_NAMED(name_, "Using saturation limits (not soft limits)"); 246 | 247 | const joint_limits_interface::PositionJointSaturationHandle sat_handle_position(joint_handle_position, joint_limits); 248 | pos_jnt_sat_interface_.registerHandle(sat_handle_position); 249 | 250 | const joint_limits_interface::VelocityJointSaturationHandle sat_handle_velocity(joint_handle_velocity, joint_limits); 251 | vel_jnt_sat_interface_.registerHandle(sat_handle_velocity); 252 | 253 | const joint_limits_interface::EffortJointSaturationHandle sat_handle_effort(joint_handle_effort, joint_limits); 254 | eff_jnt_sat_interface_.registerHandle(sat_handle_effort); 255 | } 256 | } 257 | 258 | void GenericHWInterface::reset() 259 | { 260 | // Reset joint limits state, in case of mode switch or e-stop 261 | pos_jnt_sat_interface_.reset(); 262 | pos_jnt_soft_limits_.reset(); 263 | } 264 | 265 | void GenericHWInterface::printState() 266 | { 267 | // WARNING: THIS IS NOT REALTIME SAFE 268 | // FOR DEBUGGING ONLY, USE AT YOUR OWN ROBOT's RISK! 269 | ROS_INFO_STREAM_THROTTLE(1, std::endl << printStateHelper()); 270 | } 271 | 272 | std::string GenericHWInterface::printStateHelper() 273 | { 274 | std::stringstream ss; 275 | std::cout.precision(15); 276 | 277 | for (std::size_t i = 0; i < num_joints_; ++i) 278 | { 279 | ss << "j" << i << ": " << std::fixed << joint_position_[i] << "\t "; 280 | ss << std::fixed << joint_velocity_[i] << "\t "; 281 | ss << std::fixed << joint_effort_[i] << std::endl; 282 | } 283 | return ss.str(); 284 | } 285 | 286 | std::string GenericHWInterface::printCommandHelper() 287 | { 288 | std::stringstream ss; 289 | std::cout.precision(15); 290 | ss << " position velocity effort \n"; 291 | for (std::size_t i = 0; i < num_joints_; ++i) 292 | { 293 | ss << "j" << i << ": " << std::fixed << joint_position_command_[i] << "\t "; 294 | ss << std::fixed << joint_velocity_command_[i] << "\t "; 295 | ss << std::fixed << joint_effort_command_[i] << std::endl; 296 | } 297 | return ss.str(); 298 | } 299 | 300 | void GenericHWInterface::loadURDF(const ros::NodeHandle& nh, std::string param_name) 301 | { 302 | std::string urdf_string; 303 | urdf_model_ = new urdf::Model(); 304 | 305 | // search and wait for robot_description on param server 306 | while (urdf_string.empty() && ros::ok()) 307 | { 308 | std::string search_param_name; 309 | if (nh.searchParam(param_name, search_param_name)) 310 | { 311 | ROS_INFO_STREAM_NAMED(name_, "Waiting for model URDF on the ROS param server at location: " << nh.getNamespace() 312 | << search_param_name); 313 | nh.getParam(search_param_name, urdf_string); 314 | } 315 | else 316 | { 317 | ROS_INFO_STREAM_NAMED(name_, "Waiting for model URDF on the ROS param server at location: " << nh.getNamespace() 318 | << param_name); 319 | nh.getParam(param_name, urdf_string); 320 | } 321 | 322 | usleep(100000); 323 | } 324 | 325 | if (!urdf_model_->initString(urdf_string)) 326 | ROS_ERROR_STREAM_NAMED(name_, "Unable to load URDF model"); 327 | else 328 | ROS_DEBUG_STREAM_NAMED(name_, "Received URDF from param server"); 329 | } 330 | 331 | } // namespace ros_control_boilerplate 332 | -------------------------------------------------------------------------------- /src/sim_hw_interface.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Example ros_control hardware interface that performs a perfect control loop for 37 | simulation 38 | */ 39 | 40 | #include 41 | 42 | // ROS parameter loading 43 | #include 44 | 45 | namespace ros_control_boilerplate 46 | { 47 | SimHWInterface::SimHWInterface(ros::NodeHandle& nh, urdf::Model* urdf_model) 48 | : GenericHWInterface(nh, urdf_model), name_("sim_hw_interface") 49 | { 50 | // Load rosparams 51 | ros::NodeHandle rpnh(nh_, "hardware_interface"); 52 | std::size_t error = 0; 53 | error += !rosparam_shortcuts::get(name_, rpnh, "sim_control_mode", sim_control_mode_); 54 | if (error) 55 | { 56 | ROS_WARN_STREAM_NAMED(name_, "SimHWInterface now requires the following config in the yaml:"); 57 | ROS_WARN_STREAM_NAMED(name_, " sim_control_mode: 0 # 0: position, 1: velocity"); 58 | } 59 | rosparam_shortcuts::shutdownIfError(name_, error); 60 | } 61 | 62 | void SimHWInterface::init() 63 | { 64 | // Call parent class version of this function 65 | GenericHWInterface::init(); 66 | 67 | // Resize vectors 68 | joint_position_prev_.resize(num_joints_, 0.0); 69 | 70 | ROS_INFO_NAMED(name_, "SimHWInterface Ready."); 71 | } 72 | 73 | void SimHWInterface::read(ros::Duration& elapsed_time) 74 | { 75 | // No need to read since our write() command populates our state for us 76 | } 77 | 78 | void SimHWInterface::write(ros::Duration& elapsed_time) 79 | { 80 | // Safety 81 | enforceLimits(elapsed_time); 82 | 83 | // NOTE: the following is a "simuation" example so that this boilerplate can be run without being 84 | // connected to hardware 85 | // When converting to your robot, remove the built-in PID loop and instead let the higher leverl 86 | // ros_control controllers take 87 | // care of PID loops for you. This P-controller is only intended to mimic the delay in real 88 | // hardware, somewhat like a simualator 89 | for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id) 90 | { 91 | switch (sim_control_mode_) 92 | { 93 | case 0: // hardware_interface::MODE_POSITION: 94 | positionControlSimulation(elapsed_time, joint_id); 95 | break; 96 | 97 | case 1: // hardware_interface::MODE_VELOCITY: 98 | 99 | // // Move all the states to the commanded set points slowly 100 | // joint_position_[joint_id] += joint_velocity_[joint_id] * elapsed_time.toSec(); 101 | 102 | // v_error_ = joint_velocity_command_[joint_id] - joint_velocity_[joint_id]; 103 | 104 | // // scale the rate it takes to achieve velocity by a factor that is invariant to the feedback loop 105 | // joint_velocity_[joint_id] += v_error_ * VELOCITY_STEP_FACTOR; 106 | 107 | // Naive 108 | joint_velocity_[joint_id] = joint_velocity_command_[joint_id]; 109 | joint_position_[joint_id] += joint_velocity_command_[joint_id] * elapsed_time.toSec(); 110 | 111 | break; 112 | 113 | case 2: // hardware_interface::MODE_EFFORT: 114 | ROS_ERROR_STREAM_NAMED(name_, "Effort not implemented yet"); 115 | break; 116 | } 117 | } 118 | } 119 | 120 | void SimHWInterface::enforceLimits(ros::Duration& period) 121 | { 122 | // Enforces position and velocity 123 | pos_jnt_sat_interface_.enforceLimits(period); 124 | } 125 | 126 | void SimHWInterface::positionControlSimulation(ros::Duration& elapsed_time, const std::size_t joint_id) 127 | { 128 | const double max_delta_pos = joint_velocity_limits_[joint_id] * elapsed_time.toSec(); 129 | 130 | // Move all the states to the commanded set points at max velocity 131 | p_error_ = joint_position_command_[joint_id] - joint_position_[joint_id]; 132 | 133 | const double delta_pos = std::max(std::min(p_error_, max_delta_pos), -max_delta_pos); 134 | joint_position_[joint_id] += delta_pos; 135 | 136 | // Bypass max velocity p controller: 137 | // joint_position_[joint_id] = joint_position_command_[joint_id]; 138 | 139 | // Calculate velocity based on change in positions 140 | if (elapsed_time.toSec() > 0) 141 | { 142 | joint_velocity_[joint_id] = (joint_position_[joint_id] - joint_position_prev_[joint_id]) / elapsed_time.toSec(); 143 | } 144 | else 145 | joint_velocity_[joint_id] = 0; 146 | 147 | // Save last position 148 | joint_position_prev_[joint_id] = joint_position_[joint_id]; 149 | } 150 | 151 | } // namespace ros_control_boilerplate 152 | -------------------------------------------------------------------------------- /src/sim_hw_main.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Example ros_control main() entry point for controlling robots in ROS 37 | */ 38 | 39 | #include 40 | #include 41 | 42 | int main(int argc, char** argv) 43 | { 44 | ros::init(argc, argv, "sim_hw_interface"); 45 | ros::NodeHandle nh; 46 | 47 | // NOTE: We run the ROS loop in a separate thread as external calls such 48 | // as service callbacks to load controllers can block the (main) control loop 49 | ros::AsyncSpinner spinner(3); 50 | spinner.start(); 51 | 52 | // Create the hardware interface specific to your robot 53 | std::shared_ptr sim_hw_interface( 54 | new ros_control_boilerplate::SimHWInterface(nh)); 55 | sim_hw_interface->init(); 56 | 57 | // Start the control loop 58 | ros_control_boilerplate::GenericHWControlLoop control_loop(nh, sim_hw_interface); 59 | control_loop.run(); // Blocks until shutdown signal recieved 60 | 61 | return 0; 62 | } 63 | -------------------------------------------------------------------------------- /src/tools/controller_to_csv.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Records a ros_control ControllerState data to CSV for Matlab/etc analysis 37 | */ 38 | 39 | #include 40 | 41 | // Basic file operations 42 | #include 43 | #include 44 | 45 | // ROS parameter loading 46 | #include 47 | 48 | namespace ros_control_boilerplate 49 | { 50 | ControllerToCSV::ControllerToCSV(const std::string& topic) : nh_("~"), first_update_(true), recording_started_(true) 51 | { 52 | // Load rosparams 53 | ros::NodeHandle rpsnh(nh_, name_); 54 | int error = 0; 55 | error += !rosparam_shortcuts::get(name_, rpsnh, "record_hz", record_hz_); 56 | rosparam_shortcuts::shutdownIfError(name_, error); 57 | 58 | ROS_INFO_STREAM_NAMED(name_, "Subscribing to " << topic); 59 | // State subscriber 60 | state_sub_ = nh_.subscribe(topic, 1, &ControllerToCSV::stateCB, this); 61 | 62 | // Wait for states to populate 63 | waitForSubscriber(state_sub_); 64 | 65 | // Alert user to mode 66 | if (recordAll()) 67 | { 68 | ROS_INFO_STREAM_NAMED(name_, "Recording all incoming controller state messages"); 69 | } 70 | else 71 | { 72 | ROS_INFO_STREAM_NAMED(name_, "Only recording every " << record_hz_ << " hz"); 73 | } 74 | 75 | ROS_INFO_STREAM_NAMED(name_, "ControllerToCSV Ready."); 76 | } 77 | 78 | ControllerToCSV::~ControllerToCSV() 79 | { 80 | stopRecording(); 81 | } 82 | 83 | bool ControllerToCSV::recordAll() 84 | { 85 | return record_hz_ == 0; 86 | } 87 | 88 | void ControllerToCSV::stateCB(const control_msgs::JointTrajectoryControllerState::ConstPtr& state) 89 | { 90 | // Two modes - save all immediately, or only save at certain frequency 91 | if (recordAll()) 92 | { 93 | // Record state 94 | states_.push_back(current_state_); 95 | 96 | // Record current time 97 | timestamps_.push_back(ros::Time::now()); 98 | } 99 | else // only record at freq 100 | { 101 | current_state_ = *state; 102 | } 103 | } 104 | 105 | // Start the data collection 106 | void ControllerToCSV::startRecording(const std::string& file_name) 107 | { 108 | ROS_INFO_STREAM_NAMED(name_, "Saving to " << file_name); 109 | file_name_ = file_name; 110 | 111 | // Reset data collections 112 | states_.clear(); 113 | timestamps_.clear(); 114 | 115 | recording_started_ = true; 116 | 117 | // Start sampling loop 118 | if (!recordAll()) // only record at the specified frequency 119 | { 120 | ros::Duration update_freq = ros::Duration(1.0 / record_hz_); 121 | non_realtime_loop_ = nh_.createTimer(update_freq, &ControllerToCSV::update, this); 122 | } 123 | } 124 | 125 | void ControllerToCSV::update(const ros::TimerEvent& event) 126 | { 127 | if (first_update_) 128 | { 129 | // Check if we've recieved any states yet 130 | if (current_state_.joint_names.empty()) 131 | { 132 | ROS_WARN_STREAM_THROTTLE_NAMED(2, name_, "No states recieved yet"); 133 | return; 134 | } 135 | first_update_ = false; 136 | } 137 | else // if (event.last_real > 0) 138 | { 139 | const double freq = 1.0 / (event.current_real - event.last_real).toSec(); 140 | ROS_INFO_STREAM_THROTTLE_NAMED(2, name_, 141 | "Updating at " << freq << " hz, total: " 142 | << (ros::Time::now() - timestamps_.front()).toSec() << " seconds"); 143 | } 144 | // Record state 145 | states_.push_back(current_state_); 146 | 147 | // Record current time 148 | timestamps_.push_back(ros::Time::now()); 149 | } 150 | 151 | void ControllerToCSV::stopRecording() 152 | { 153 | non_realtime_loop_.stop(); 154 | writeToFile(); 155 | } 156 | 157 | bool ControllerToCSV::writeToFile() 158 | { 159 | std::cout << "Writing data to file " << std::endl; 160 | 161 | if (!states_.size()) 162 | { 163 | std::cout << "No controller states populated" << std::endl; 164 | return false; 165 | } 166 | 167 | std::ofstream output_file; 168 | output_file.open(file_name_.c_str()); 169 | 170 | // Output header ------------------------------------------------------- 171 | output_file << "timestamp,"; 172 | for (std::size_t j = 0; j < states_[0].joint_names.size(); ++j) 173 | { 174 | output_file << states_[0].joint_names[j] << "_desired_pos," << states_[0].joint_names[j] << "_desired_vel," 175 | << states_[0].joint_names[j] << "_actual_pos," << states_[0].joint_names[j] << "_actual_vel," 176 | << states_[0].joint_names[j] << "_error_pos," << states_[0].joint_names[j] << "_error_vel,"; 177 | } 178 | output_file << std::endl; 179 | 180 | // Output data ------------------------------------------------------ 181 | 182 | // Subtract starting time 183 | // double start_time = states_[0].header.stamp.toSec(); 184 | double start_time = timestamps_[0].toSec(); 185 | 186 | for (std::size_t i = 0; i < states_.size(); ++i) 187 | { 188 | // Timestamp 189 | output_file << timestamps_[i].toSec() - start_time << ","; 190 | 191 | // Output entire state of robot to single line 192 | for (std::size_t j = 0; j < states_[i].joint_names.size(); ++j) 193 | { 194 | // Output State 195 | output_file << states_[i].desired.positions[j] << "," << states_[i].desired.velocities[j] << "," 196 | << states_[i].actual.positions[j] << "," << states_[i].actual.velocities[j] << "," 197 | << states_[i].error.positions[j] << "," << states_[i].error.velocities[j] << ","; 198 | } 199 | 200 | output_file << std::endl; 201 | } 202 | output_file.close(); 203 | std::cout << "Wrote to file: " << file_name_ << std::endl; 204 | return true; 205 | } 206 | 207 | bool ControllerToCSV::waitForSubscriber(const ros::Subscriber& sub, const double& wait_time) 208 | { 209 | // Benchmark runtime 210 | ros::Time start_time; 211 | start_time = ros::Time::now(); 212 | 213 | // Will wait at most 1000 ms (1 sec) 214 | ros::Time max_time(ros::Time::now() + ros::Duration(wait_time)); 215 | 216 | // This is wrong. It returns only the number of subscribers that have already established their 217 | // direct connections to this subscriber 218 | int num_existing_subscribers = sub.getNumPublishers(); 219 | 220 | // How often to check for subscribers 221 | ros::Rate poll_rate(200); 222 | 223 | // Wait for subsriber 224 | while (num_existing_subscribers == 0) 225 | { 226 | // Check if timed out 227 | if (ros::Time::now() > max_time) 228 | { 229 | ROS_WARN_STREAM_NAMED(name_, "Topic '" << sub.getTopic() << "' unable to connect to any publishers within " 230 | << wait_time << " seconds."); 231 | return false; 232 | } 233 | ros::spinOnce(); 234 | 235 | // Sleep 236 | poll_rate.sleep(); 237 | 238 | // Check again 239 | num_existing_subscribers = sub.getNumPublishers(); 240 | } 241 | 242 | // Benchmark runtime 243 | if (true) 244 | { 245 | double duration = (ros::Time::now() - start_time).toSec(); 246 | ROS_DEBUG_STREAM_NAMED(name_, "Topic '" << sub.getTopic() << "' took " << duration 247 | << " seconds to connect to a subscriber. " 248 | "Connected to " 249 | << num_existing_subscribers << " total subsribers"); 250 | } 251 | return true; 252 | } 253 | 254 | } // namespace ros_control_boilerplate 255 | -------------------------------------------------------------------------------- /src/tools/controller_to_csv_main.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Records ros_control ControllerState data to CSV for Matlab/etc analysis 37 | */ 38 | 39 | #include 40 | 41 | // Command line arguments 42 | #include 43 | 44 | DEFINE_string(csv_path, "/tmp/recorded_trajectory_1.csv", "File location to save recoded data to"); 45 | DEFINE_string(topic, "/robot/position_trajectory_controller/state", "ROS topic to subscribe to"); 46 | 47 | int main(int argc, char** argv) 48 | { 49 | google::SetVersionString("0.0.1"); 50 | google::SetUsageMessage("Utility to record controller topic to a CSV"); 51 | google::ParseCommandLineFlags(&argc, &argv, true); 52 | 53 | ros::init(argc, argv, "controller_to_csv", ros::init_options::AnonymousName); 54 | ROS_INFO_STREAM_NAMED("main", "Starting ControllerToCSV..."); 55 | 56 | // Allow the action server to recieve and send ros messages 57 | ros::AsyncSpinner spinner(2); 58 | spinner.start(); 59 | 60 | const std::string topic = FLAGS_topic; 61 | const std::string csv_path = FLAGS_csv_path; 62 | ros_control_boilerplate::ControllerToCSV converter(topic); 63 | converter.startRecording(csv_path); 64 | 65 | ROS_INFO_STREAM_NAMED("main", "Type Ctrl-C to end and save"); 66 | ros::spin(); 67 | 68 | ROS_INFO_STREAM_NAMED("main", "Shutting down."); 69 | ros::shutdown(); 70 | 71 | return 0; 72 | } 73 | -------------------------------------------------------------------------------- /src/tools/csv_to_controller.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Records a ros_control ControllerState data to CSV for Matlab/etc analysis 37 | */ 38 | 39 | #include 40 | 41 | // Basic file operations 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | namespace ros_control_boilerplate 51 | { 52 | CSVToController::CSVToController(const std::string& joint_trajectory_action, const std::string& controller_state_topic) 53 | : joint_trajectory_action_(joint_trajectory_action), controller_state_topic_(controller_state_topic) 54 | { 55 | ROS_INFO_STREAM_NAMED("csv_to_controller", "Waiting for action server"); 56 | joint_trajectory_action_.waitForServer(); 57 | 58 | // State subscriber 59 | state_sub_ = nh_.subscribe(controller_state_topic_, 1, 60 | &CSVToController::stateCB, this); 61 | 62 | // Wait for states to populate 63 | ros::spinOnce(); 64 | ros::Duration(0.5).sleep(); 65 | 66 | ROS_INFO_STREAM_NAMED("csv_to_controller", "CSVToController Ready."); 67 | } 68 | 69 | void CSVToController::stateCB(const control_msgs::JointTrajectoryControllerState::ConstPtr& state) 70 | { 71 | current_state_ = *state; 72 | } 73 | 74 | void CSVToController::printPoint(trajectory_msgs::JointTrajectoryPoint& point) 75 | { 76 | // Show new line 77 | std::copy(point.positions.begin(), point.positions.end(), std::ostream_iterator(std::cout, " ")); 78 | std::cout << std::endl; 79 | } 80 | 81 | // Start the data collection 82 | void CSVToController::loadAndRunCSV(const std::string& file_name) 83 | { 84 | file_name_ = file_name; 85 | 86 | // Open file 87 | std::ifstream input_file; 88 | input_file.open(file_name_.c_str()); 89 | 90 | // Settings 91 | std::string line; 92 | std::string cell; 93 | 94 | control_msgs::FollowJointTrajectoryGoal pre_goal; // moving to start state 95 | control_msgs::FollowJointTrajectoryGoal goal; // csv file 96 | 97 | // Populate joint names 98 | goal.trajectory.joint_names.push_back("joint_a1"); 99 | goal.trajectory.joint_names.push_back("joint_a2"); 100 | goal.trajectory.joint_names.push_back("joint_a3"); 101 | goal.trajectory.joint_names.push_back("joint_a4"); 102 | goal.trajectory.joint_names.push_back("joint_a5"); 103 | goal.trajectory.joint_names.push_back("joint_a6"); 104 | goal.trajectory.joint_names.push_back("joint_a7"); 105 | pre_goal.trajectory.joint_names = goal.trajectory.joint_names; 106 | double num_joints = goal.trajectory.joint_names.size(); 107 | 108 | // Skip header 109 | std::getline(input_file, line); 110 | 111 | // For each line/row 112 | while (std::getline(input_file, line)) 113 | { 114 | std::stringstream lineStream(line); 115 | 116 | trajectory_msgs::JointTrajectoryPoint point; 117 | 118 | // TIME FROM START 119 | if (!std::getline(lineStream, cell, ',')) 120 | ROS_ERROR_STREAM_NAMED("csv_to_controller", "no joint value"); 121 | 122 | point.time_from_start = ros::Duration(atof(cell.c_str())); 123 | 124 | // For each item/column 125 | for (std::size_t i = 0; i < num_joints; ++i) 126 | { 127 | // DESIRED POSITION 128 | if (!std::getline(lineStream, cell, ',')) 129 | ROS_ERROR_STREAM_NAMED("csv_to_controller", "no joint value"); 130 | // UNUSED 131 | 132 | // DESIRED VELOCITY 133 | if (!std::getline(lineStream, cell, ',')) 134 | ROS_ERROR_STREAM_NAMED("csv_to_controller", "no joint value"); 135 | // UNUSED 136 | 137 | // ACTUAL POSITION 138 | if (!std::getline(lineStream, cell, ',')) 139 | ROS_ERROR_STREAM_NAMED("csv_to_controller", "no joint value"); 140 | point.positions.push_back(atof(cell.c_str())); 141 | 142 | // ACTUAL VELOCITY 143 | if (!std::getline(lineStream, cell, ',')) 144 | ROS_ERROR_STREAM_NAMED("csv_to_controller", "no joint value"); 145 | point.velocities.push_back(atof(cell.c_str())); 146 | 147 | // COMMANDED VEL 148 | if (!std::getline(lineStream, cell, ',')) 149 | ROS_ERROR_STREAM_NAMED("csv_to_controller", "no joint value"); 150 | // UNUSED 151 | } 152 | 153 | goal.trajectory.points.push_back(point); 154 | } // while 155 | 156 | // Check that we have a current state 157 | if (current_state_.actual.positions.empty()) 158 | { 159 | ROS_ERROR_STREAM_NAMED("csv_to_controller", "Unable to find current state msg"); 160 | return; 161 | } 162 | // Add current state to start of trajectory 163 | trajectory_msgs::JointTrajectoryPoint last_point; 164 | last_point.positions = current_state_.actual.positions; 165 | last_point.velocities = current_state_.actual.velocities; 166 | 167 | std::cout << "Current State:" << std::endl; 168 | printPoint(last_point); 169 | printPoint(goal.trajectory.points.front()); 170 | std::cout << "^^ Goal point " << std::endl; 171 | pre_goal.trajectory.points.push_back(last_point); 172 | 173 | // Interpolate from first point 174 | bool done = false; 175 | double max_velocity = 0.1; // m/s or radians/s 176 | double frequency = 200; // hz 177 | double q_delta = max_velocity / frequency; // m 178 | double t_delta = 1 / frequency; 179 | ros::Duration time_from_start(1); 180 | while (!done) 181 | { 182 | done = true; 183 | trajectory_msgs::JointTrajectoryPoint new_point = last_point; 184 | 185 | // Time change 186 | time_from_start += ros::Duration(t_delta); 187 | new_point.time_from_start = time_from_start; 188 | 189 | // Position change 190 | for (std::size_t i = 0; i < num_joints; ++i) // each joint 191 | { 192 | // Do we need to move this joint foward? 193 | if (new_point.positions[i] < goal.trajectory.points.front().positions[i]) 194 | { 195 | // Do not allow to go past goal point 196 | new_point.positions[i] = 197 | std::min(new_point.positions[i] + q_delta, goal.trajectory.points.front().positions[i]); 198 | new_point.velocities[i] = max_velocity; 199 | done = false; 200 | } 201 | else 202 | { 203 | // Maintain velocity 204 | new_point.velocities[i] = 0; 205 | } 206 | } 207 | pre_goal.trajectory.points.push_back(new_point); 208 | last_point = new_point; 209 | 210 | printPoint(new_point); 211 | } 212 | 213 | // std::cout << "TRAJECTORY: " << trajectory << std::endl; 214 | // std::cout << "TRAJECTORY: " << goal.trajectory << std::endl; 215 | 216 | ROS_INFO_STREAM_NAMED("temp", "Sleeping for " << time_from_start.toSec()); 217 | pre_goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(0.5); 218 | joint_trajectory_action_.sendGoal(pre_goal); 219 | ros::Duration(time_from_start).sleep(); 220 | 221 | for (std::size_t i = 0; i < goal.trajectory.points.size(); ++i) 222 | { 223 | printPoint(goal.trajectory.points[i]); 224 | } 225 | 226 | ROS_INFO_STREAM_NAMED("csv_to_controller", "Preparing to follow CSV path"); 227 | ros::Duration(0.5).sleep(); 228 | goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(0.5); 229 | joint_trajectory_action_.sendGoal(goal); 230 | } 231 | 232 | } // namespace ros_control_boilerplate 233 | -------------------------------------------------------------------------------- /src/tools/csv_to_controller_main.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Records ros_control ControllerState datas to CSV for Matlab/etc analysis 37 | */ 38 | 39 | #include 40 | 41 | // Command line arguments 42 | #include 43 | 44 | DEFINE_string(csv_path, "", "File location to load recoded data from"); 45 | DEFINE_string(joint_trajectory_action, "", "Which action server to send commands to"); 46 | DEFINE_string(controller_state_topic, "", "Where to subscribe the controller state"); 47 | 48 | int main(int argc, char** argv) 49 | { 50 | google::SetVersionString("0.0.1"); 51 | google::SetUsageMessage("Utility to load commands from a CSV"); 52 | google::ParseCommandLineFlags(&argc, &argv, true); 53 | 54 | ros::init(argc, argv, "csv_to_controller"); 55 | ROS_INFO_STREAM_NAMED("main", "Starting CSVToController..."); 56 | 57 | // Get file name 58 | if (FLAGS_csv_path.empty()) 59 | { 60 | ROS_ERROR_STREAM_NAMED("csv_to_controller", "No file name passed in"); 61 | return 0; 62 | } 63 | ROS_INFO_STREAM_NAMED("csv_to_controller", "Reading from file " << FLAGS_csv_path); 64 | 65 | // Allow the action server to recieve and send ros messages 66 | ros::AsyncSpinner spinner(2); 67 | spinner.start(); 68 | 69 | ros_control_boilerplate::CSVToController converter(FLAGS_joint_trajectory_action, FLAGS_controller_state_topic); 70 | converter.loadAndRunCSV(FLAGS_csv_path); 71 | 72 | ROS_INFO_STREAM_NAMED("main", "Shutting down."); 73 | ros::shutdown(); 74 | 75 | return 0; 76 | } 77 | -------------------------------------------------------------------------------- /src/tools/keyboard_teleop.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, PickNik LLC 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the PickNik LLC nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman , Andy McEvoy 36 | Desc: Tweak a TF transform using a keyboard 37 | */ 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | #include 47 | 48 | #define KEYCODE_a 0x61 49 | #define KEYCODE_b 0x62 50 | #define KEYCODE_c 0x63 51 | #define KEYCODE_d 0x64 52 | #define KEYCODE_e 0x65 53 | #define KEYCODE_f 0x66 54 | #define KEYCODE_g 0x67 55 | #define KEYCODE_h 0x68 56 | #define KEYCODE_i 0x69 57 | #define KEYCODE_j 0x6a 58 | #define KEYCODE_k 0x6b 59 | #define KEYCODE_l 0x6c 60 | #define KEYCODE_m 0x6d 61 | #define KEYCODE_n 0x6e 62 | #define KEYCODE_o 0x6f 63 | #define KEYCODE_p 0x70 64 | #define KEYCODE_q 0x71 65 | #define KEYCODE_r 0x72 66 | #define KEYCODE_s 0x73 67 | #define KEYCODE_t 0x74 68 | #define KEYCODE_u 0x75 69 | #define KEYCODE_v 0x76 70 | #define KEYCODE_w 0x77 71 | #define KEYCODE_x 0x78 72 | #define KEYCODE_y 0x79 73 | #define KEYCODE_z 0x7a 74 | #define KEYCODE_ESCAPE 0x1B 75 | 76 | int kfd = 0; 77 | struct termios cooked, raw; 78 | 79 | void quit(int sig) 80 | { 81 | tcsetattr(kfd, TCSANOW, &cooked); 82 | exit(0); 83 | } 84 | 85 | class TeleopJointsKeyboard 86 | { 87 | public: 88 | TeleopJointsKeyboard() : has_recieved_joints_(false) 89 | { 90 | std::cout << "init " << std::endl; 91 | // TODO: make this robot agonistic 92 | joints_sub_ = nh_.subscribe("/iiwa_7_r800/joint_states", 1, 93 | &TeleopJointsKeyboard::stateCallback, this); 94 | joints_pub_ = nh_.advertise("/iiwa_7_r800/joints_position_controller/command", 1); 95 | cmd_.data.resize(7); 96 | } 97 | 98 | ~TeleopJointsKeyboard() 99 | { 100 | } 101 | 102 | void stateCallback(const sensor_msgs::JointStateConstPtr& msg) 103 | { 104 | if (msg->position.size() != 7) 105 | { 106 | ROS_ERROR_STREAM("Not enough joints!"); 107 | exit(-1); 108 | } 109 | 110 | // Copy latest joint positions to our output message 111 | if (!has_recieved_joints_) 112 | cmd_.data = msg->position; 113 | 114 | // Debug 115 | // std::copy(cmd_.data.begin(), cmd_.data.end(), std::ostream_iterator(std::cout, " ")); 116 | // std::cout << std::endl; 117 | 118 | // Important safety feature 119 | has_recieved_joints_ = true; 120 | } 121 | 122 | void keyboardLoop() 123 | { 124 | char c; 125 | bool dirty = false; 126 | 127 | // get the console in raw mode 128 | tcgetattr(kfd, &cooked); 129 | memcpy(&raw, &cooked, sizeof(struct termios)); 130 | raw.c_lflag &= ~(ICANON | ECHO); 131 | // Setting a new line, then end of file 132 | raw.c_cc[VEOL] = 1; 133 | raw.c_cc[VEOF] = 2; 134 | tcsetattr(kfd, TCSANOW, &raw); 135 | 136 | puts("Reading from keyboard"); 137 | puts("---------------------------"); 138 | puts("Use 'QA' to for joint 1"); 139 | puts("Use 'WS' to for joint 2"); 140 | puts("Use 'ED' to for joint 3"); 141 | puts("Use 'RF' to for joint 4"); 142 | puts("Use 'TG' to for joint 5"); 143 | puts("Use 'YH' to for joint 6"); 144 | puts("Use 'UJ' to for joint 7"); 145 | puts("ESC to end"); 146 | 147 | double delta_dist = 0.005; 148 | 149 | for (;;) 150 | { 151 | // get the next event from the keyboard 152 | if (read(kfd, &c, 1) < 0) 153 | { 154 | perror("read():"); 155 | exit(-1); 156 | } 157 | 158 | dirty = true; 159 | switch (c) 160 | { 161 | case KEYCODE_q: 162 | cmd_.data[0] = cmd_.data[0] + delta_dist; // radians 163 | break; 164 | case KEYCODE_a: 165 | cmd_.data[0] = cmd_.data[0] - delta_dist; // radians 166 | break; 167 | 168 | case KEYCODE_w: 169 | cmd_.data[1] = cmd_.data[1] + delta_dist; // radians 170 | break; 171 | case KEYCODE_s: 172 | cmd_.data[1] = cmd_.data[1] - delta_dist; // radians 173 | break; 174 | 175 | case KEYCODE_e: 176 | cmd_.data[2] = cmd_.data[2] + delta_dist; // radians 177 | break; 178 | case KEYCODE_d: 179 | cmd_.data[2] = cmd_.data[2] - delta_dist; // radians 180 | break; 181 | 182 | case KEYCODE_r: 183 | cmd_.data[3] = cmd_.data[3] + delta_dist; // radians 184 | break; 185 | case KEYCODE_f: 186 | cmd_.data[3] = cmd_.data[3] - delta_dist; // radians 187 | break; 188 | 189 | case KEYCODE_t: 190 | cmd_.data[4] = cmd_.data[4] + delta_dist; // radians 191 | break; 192 | case KEYCODE_g: 193 | cmd_.data[4] = cmd_.data[4] - delta_dist; // radians 194 | break; 195 | 196 | case KEYCODE_y: 197 | cmd_.data[5] = cmd_.data[5] + delta_dist; // radians 198 | break; 199 | case KEYCODE_h: 200 | cmd_.data[5] = cmd_.data[5] - delta_dist; // radians 201 | break; 202 | 203 | case KEYCODE_u: 204 | cmd_.data[6] = cmd_.data[6] + delta_dist; // radians 205 | break; 206 | case KEYCODE_j: 207 | cmd_.data[6] = cmd_.data[6] - delta_dist; // radians 208 | break; 209 | 210 | case KEYCODE_ESCAPE: 211 | std::cout << std::endl; 212 | std::cout << "Exiting " << std::endl; 213 | quit(0); 214 | break; 215 | 216 | default: 217 | std::cout << "CODE: " << c << std::endl; 218 | dirty = false; 219 | } 220 | 221 | // Publish command 222 | if (dirty) 223 | { 224 | // Important safety feature 225 | if (!has_recieved_joints_) 226 | { 227 | ROS_ERROR_STREAM_NAMED("joint_teleop", "Unable to send joint commands because robot state is invalid"); 228 | } 229 | else 230 | { 231 | std::cout << "."; 232 | joints_pub_.publish(cmd_); 233 | } 234 | } 235 | } 236 | } 237 | 238 | private: 239 | ros::NodeHandle nh_; 240 | ros::Publisher joints_pub_; 241 | ros::Subscriber joints_sub_; 242 | std_msgs::Float64MultiArray cmd_; 243 | bool has_recieved_joints_; 244 | }; 245 | 246 | int main(int argc, char** argv) 247 | { 248 | ros::init(argc, argv, "joints_teleop_keyboard"); 249 | signal(SIGINT, quit); 250 | 251 | // NOTE: We run the ROS loop in a separate thread as external calls such 252 | // as service callbacks to load controllers can block the (main) control loop 253 | ros::AsyncSpinner spinner(1); 254 | spinner.start(); 255 | 256 | TeleopJointsKeyboard teleop; 257 | teleop.keyboardLoop(); 258 | 259 | return (0); 260 | } 261 | -------------------------------------------------------------------------------- /src/tools/test_trajectory.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Generate a random trajectory to test the ros_control controller 37 | */ 38 | 39 | // ROS 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | namespace ros_control_boilerplate 47 | { 48 | static const double SEC_PER_TRAJ_POINT = 5.0; // time between points 49 | static const std::size_t TRAJ_POINTS = 10; // number of points to generate 50 | 51 | class TestTrajectory 52 | { 53 | public: 54 | /** 55 | * \brief Constructor 56 | */ 57 | TestTrajectory() : nh_private_("~") 58 | { 59 | nh_private_.getParam("trajectory_controller", trajectory_controller); 60 | if (trajectory_controller.empty()) 61 | { 62 | ROS_FATAL_STREAM_NAMED("test_trajectory", 63 | "No joint trajectory controller parameter found on the parameter server"); 64 | exit(-1); 65 | } 66 | ROS_INFO_STREAM_NAMED("test_trajectory", "Connecting to controller " << trajectory_controller); 67 | 68 | // create the action client 69 | // true causes the client to spin its own thread 70 | actionlib::SimpleActionClient action_client( 71 | trajectory_controller + "/follow_joint_trajectory/", true); 72 | 73 | ROS_INFO_NAMED("test_trajetory", "Waiting for action server to start."); 74 | // wait for the action server to start 75 | action_client.waitForServer(); // will wait for infinite time 76 | 77 | ROS_INFO_NAMED("test_trajetory", "Action server started, sending goal."); 78 | 79 | // send a goal to the action 80 | control_msgs::FollowJointTrajectoryGoal goal; 81 | goal.trajectory = createTrajectory(); 82 | std::cout << "Trajectry:\n" << goal.trajectory << std::endl; 83 | action_client.sendGoal(goal); 84 | 85 | // Wait for the action to return 86 | double wait_extra_padding = 2; // time to wait longer than trajectory itself 87 | bool finished_before_timeout = action_client.waitForResult( 88 | ros::Duration(goal.trajectory.points.back().time_from_start.toSec() + wait_extra_padding)); 89 | 90 | if (finished_before_timeout) 91 | { 92 | actionlib::SimpleClientGoalState state = action_client.getState(); 93 | ROS_INFO_NAMED("test_trajetory", "Action finished: %s", state.toString().c_str()); 94 | } 95 | else 96 | ROS_INFO_NAMED("test_trajetory", "Action did not finish before the time out."); 97 | 98 | ROS_INFO_STREAM_NAMED("test_trajectory", "TestTrajectory Finished"); 99 | } 100 | 101 | /** 102 | * \brief Create random trajectory 103 | */ 104 | trajectory_msgs::JointTrajectory createTrajectory() 105 | { 106 | std::vector joint_names; 107 | double min_joint_value = -3.14; 108 | double max_joint_value = 3.14; 109 | 110 | // Get joint names 111 | nh_private_.getParam(trajectory_controller + "/joints", joint_names); 112 | if (joint_names.size() == 0) 113 | { 114 | ROS_FATAL_STREAM_NAMED("init", 115 | "Not joints found on parameter server for controller, did you load the proper yaml file?" 116 | << " Namespace: " << nh_private_.getNamespace() << "/hardware_interface/joints"); 117 | exit(-1); 118 | } 119 | 120 | nh_private_.getParam("min_joint_value", min_joint_value); 121 | nh_private_.getParam("max_joint_value", max_joint_value); 122 | ROS_DEBUG_STREAM_NAMED("test_trajectory", "Creating trajectory with joint values from " << min_joint_value << " to " 123 | << max_joint_value); 124 | 125 | // Create header 126 | trajectory_msgs::JointTrajectory trajectory; 127 | trajectory.header.stamp = ros::Time::now(); 128 | trajectory.joint_names = joint_names; 129 | 130 | // Create trajectory with x points 131 | trajectory.points.resize(TRAJ_POINTS); 132 | for (std::size_t i = 0; i < TRAJ_POINTS; ++i) 133 | { 134 | trajectory.points[i].positions.resize(joint_names.size()); 135 | // for each joint 136 | for (std::size_t j = 0; j < joint_names.size(); ++j) 137 | { 138 | trajectory.points[i].positions[j] = dRand(min_joint_value, max_joint_value); 139 | trajectory.points[i].time_from_start = ros::Duration(i * SEC_PER_TRAJ_POINT); 140 | } 141 | } 142 | 143 | return trajectory; 144 | } 145 | 146 | /** \brief Get random number */ 147 | double dRand(double dMin, double dMax) 148 | { 149 | double d = (double)rand() / RAND_MAX; 150 | return dMin + d * (dMax - dMin); 151 | } 152 | 153 | private: 154 | // A shared node handle 155 | ros::NodeHandle nh_private_; 156 | 157 | // A string containing the 'trajectory_controller' parameter value 158 | std::string trajectory_controller; 159 | }; // end class 160 | 161 | // Create std pointers for this class 162 | typedef std::shared_ptr TestTrajectoryPtr; 163 | typedef std::shared_ptr TestTrajectoryConstPtr; 164 | 165 | } // namespace ros_control_boilerplate 166 | 167 | int main(int argc, char** argv) 168 | { 169 | ros::init(argc, argv, "test_trajectory"); 170 | ROS_INFO_STREAM_NAMED("test_trajectory", "Starting TestTrajectory..."); 171 | 172 | // Allow the action server to recieve and send ros messages 173 | ros::AsyncSpinner spinner(2); 174 | spinner.start(); 175 | 176 | ros_control_boilerplate::TestTrajectory server; 177 | 178 | ROS_INFO_STREAM_NAMED("test_trajectory", "Shutting down."); 179 | ros::shutdown(); 180 | 181 | return 0; 182 | } 183 | --------------------------------------------------------------------------------