├── .documents └── udev │ ├── 91-third-robot.rules │ └── AboutUdev.md ├── .gitignore ├── .travis.yml ├── LICENSE ├── README.md ├── steer_bot_hardware_gazebo ├── CMakeLists.txt ├── README.md ├── config │ ├── pids.yaml │ └── steer_bot_hardware_gazebo.yaml ├── include │ └── steer_bot_hardware_gazebo │ │ └── steer_bot_hardware_gazebo.h ├── package.xml ├── path.bash ├── src │ └── steer_bot_hardware_gazebo.cpp └── steer_bot_hardware_gazebo_plugins.xml ├── steer_drive_controller ├── .fig │ ├── rocker_bogie_left.png │ └── rocker_bogie_right.png ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── steer_drive_controller │ │ ├── odometry.h │ │ ├── speed_limiter.h │ │ └── steer_drive_controller.h ├── package.xml ├── path.bash ├── src │ ├── odometry.cpp │ ├── speed_limiter.cpp │ └── steer_drive_controller.cpp ├── steer_drive_controller_plugins.xml └── test │ ├── common │ ├── config │ │ ├── steerbot_controllers.yaml │ │ └── steerbot_hw_sim.yaml │ ├── include │ │ ├── steerbot.h │ │ └── test_common.h │ ├── launch │ │ ├── steer_drive_common.launch │ │ └── view_steerbot.launch │ ├── rviz │ │ └── display.rviz │ ├── src │ │ └── steerbot.cpp │ └── urdf │ │ ├── steerbot.xacro │ │ ├── steerbot_sphere_wheels.xacro │ │ ├── wheel.xacro │ │ └── wheel_sphere.xacro │ ├── steer_drive_controller_default_odom_frame_test │ ├── steer_drive_controller_default_odom_frame.test │ └── steer_drive_controller_default_odom_frame_test.cpp │ ├── steer_drive_controller_fail_test │ ├── steer_drive_controller_fail_test.cpp │ ├── steer_drive_controller_wrong.test │ └── steerbot_wrong.yaml │ ├── steer_drive_controller_limits_test │ ├── steer_drive_controller_limits.test │ ├── steer_drive_controller_limits_test.cpp │ └── steerbot_limits.yaml │ ├── steer_drive_controller_nan_test │ ├── steer_drive_controller_nan.test │ └── steer_drive_controller_nan_test.cpp │ ├── steer_drive_controller_no_steer_test │ ├── steer_drive_controller_no_steer.test │ └── steerbot_no_steer.yaml │ ├── steer_drive_controller_no_wheel_test │ ├── steer_drive_controller_no_wheel.test │ └── steerbot_no_wheel.yaml │ ├── steer_drive_controller_odom_frame_test │ ├── steer_drive_controller_odom_frame.test │ └── steer_drive_controller_odom_frame_test.cpp │ ├── steer_drive_controller_odom_tf_test │ ├── steer_drive_controller_odom_tf.test │ └── steer_drive_controller_odom_tf_test.cpp │ ├── steer_drive_controller_open_loop_test │ ├── steer_drive_controller_open_loop.test │ └── steerbot_open_loop.yaml │ ├── steer_drive_controller_radius_param_fail_test │ └── steer_drive_controller_radius_param_fail.test │ ├── steer_drive_controller_radius_param_test │ └── steer_drive_controller_radius_param.test │ ├── steer_drive_controller_separation_param_test │ └── steer_drive_controller_separation_param.test │ ├── steer_drive_controller_test │ ├── steer_drive_controller.test │ └── steer_drive_controller_test.cpp │ └── steer_drive_controller_timeout_test │ ├── steer_drive_controller_timeout.test │ ├── steer_drive_controller_timeout_test.cpp │ └── steerbot_timeout.yaml ├── steer_drive_ros ├── CMakeLists.txt └── package.xml └── stepback_and_steerturn_recovery ├── CMakeLists.txt ├── README.md ├── config ├── backwards.yaml └── planner_sample.yaml ├── include └── stepback_and_steerturn_recovery │ └── stepback_and_steerturn_recovery.h ├── package.xml ├── path.bash ├── src └── stepback_and_steerturn_recovery.cpp └── stepback_and_steerturn_recovery_plugin.xml /.documents/udev/91-third-robot.rules: -------------------------------------------------------------------------------- 1 | # ttyACM0: Arduino, ttyACM1: LRF(bottom), ttyACM2: LRF(top), ttyACM3: LRF(rear), urbtc0: iMCs01 2 | SUBSYSTEMS=="usb", ATTRS{idProduct}=="0043", ATTRS{idVendor}=="2341", NAME="ttyACM0", MODE="0777", SYMLINK+="arduino arduino_uno_$attr{serial}" 3 | 4 | KERNEL=="ttyACM[0-9]*", ACTION=="add", ATTRS{idVendor}=="15d1", MODE="0777", GROUP="dialout", PROGRAM="/opt/ros/indigo/env.sh rosrun hokuyo_node getID %N q", SYMLINK+="sensors/hokuyo_%c" 5 | 6 | KERNEL=="urbtc*" ATTRS{idVendor}=="1d6b" ATTRS{idProduct}=="0002" MODE="0777" NAME="urbtc0" 7 | -------------------------------------------------------------------------------- /.documents/udev/AboutUdev.md: -------------------------------------------------------------------------------- 1 | # udevを使ったデバイスの名前固定と権限設定 2 | 3 | udevを使えばデバイスの名前を固定できてかつ権限まで設定できるみたいなのでちょっとやってみる備忘録。 4 | 5 | 参考URL 6 | - http://demura.net/misc/4493.html 7 | - http://mohammedari.blogspot.jp/2013/10/ubuntuurgviewertopurg.html 8 | - http://ubuntuforums.org/showthread.php?t=1265469 9 | - https://docs.oracle.com/cd/E39368_01/e48214/ch07s03.html 10 | - http://wiki.ros.org/hokuyo_node 11 | 12 | ## デバイス固有の情報を調べる 13 | 14 | ### デバイスを接続して `udevadm` を実行 15 | 16 | ここではiMCS01を例にやってみる。 17 | まず、事前にiMCs01のドライバを`insmod`しておく必要がある。このドライバはPCを再起動するたびに`insmod`する必要がある。 18 | iMCs01のドライバを`insmod`して、デバイスを接続したら、 19 | 20 | ```bash 21 | udevadm info -a -p $(udevadm info -q path -n /dev/urbtc0) 22 | ``` 23 | 24 | を実行する。すると、次のような結果が得られる。途中で略してある。 25 | 26 | ```bash 27 | looking at device '/devices/pci0000:00/0000:00:14.0/usb1/1-1/1-1.4/1-1.4:1.0/usbmisc/urbtc0': 28 | KERNEL=="urbtc0" 29 | SUBSYSTEM=="usbmisc" 30 | DRIVER=="" 31 | 32 | ... 33 | ``` 34 | 35 | 上記の実行結果から、`idVendor`と`idProduct`を探す。 36 | iMCs01だったら次のような感じでした。 37 | ```bash 38 | ATTRS{idVendor}=="1d6b" 39 | ATTRS{idProduct}=="0002" 40 | ``` 41 | 42 | これを各デバイスついて調べる。 43 | 3号機の場合だと、iMCs01の他にLRFが3台とArduinoが1台。 44 | 45 | ## `/etc/udev/rules.d/91-third-robot.rules` というファイルを作成する 46 | 47 | `91-third-robot.rules`の最初の数字は読み込まる順番を示しているらしい。 48 | 中身は次のような感じ。 49 | LRFの場合は固有の情報が `udevadm` を使って取得できないのでROSの力を使う（強い）。roswikiを参考に。 50 | 51 | ``` 52 | # ttyACM0: Arduino, ttyACM1: LRF(bottom), ttyACM2: LRF(top), ttyACM3: LRF(rear), urbtc0: iMCs01 53 | KERNEL=="ttyACM* SUBSYSTEMS=="usb" ATTRS{idVendor}=="2341" ATTRS{idProduct}=="0043" NAME="ttyACM0" MODE="0777" 54 | 55 | KERNEL=="ttyACM[0-9]*", ACTION=="add", ATTRS{idVendor}=="15d1", MODE="0777", GROUP="dialout", PROGRAM="/opt/ros/indigo/env.sh rosrun hokuyo_node getID %N q", SYMLINK+="sensors/hokuyo_%c" 56 | 57 | KERNEL=="urbtc*" ATTRS{idVendor}=="1d6b" ATTRS{idProduct}=="0002" MODE="0777" NAME="urbtc0" 58 | ``` 59 | 60 | このドキュメントと同じ階層にあるファイルをコピーしても良い。 61 | ```bash 62 | $ sudo cp /src/TC2015/third_robot/.documents/udev/91-third-robot.udev /etc/udev/rules.d/ 63 | ``` 64 | 65 | ## restart 66 | 67 | ```bash 68 | $sudo service udev restart 69 | ``` 70 | 71 | ## 挿しなおしてみたりする 72 | 73 | 権限とかもちゃんと設定できてると思う。 74 | ```bash 75 | ls -l /dev/sensors/ 76 | lrwxrwxrwx 1 root root 10 10月 12 19:13 hokuyo_H1101524 -> ../ttyACM2 # top 77 | lrwxrwxrwx 1 root root 10 10月 12 19:12 hokuyo_H1101683 -> ../ttyACM1 # bottom 78 | ``` 79 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | bin/ 3 | lib/ 4 | msg_gen/ 5 | srv_gen/ 6 | msg/*Action.msg 7 | msg/*ActionFeedback.msg 8 | msg/*ActionGoal.msg 9 | msg/*ActionResult.msg 10 | msg/*Feedback.msg 11 | msg/*Goal.msg 12 | msg/*Result.msg 13 | msg/_*.py 14 | 15 | # Generated by dynamic reconfigure 16 | *.cfgc 17 | /cfg/cpp/ 18 | /cfg/*.py 19 | 20 | # Ignore generated docs 21 | *.dox 22 | *.wikidoc 23 | 24 | # eclipse stuff 25 | .project 26 | .cproject 27 | 28 | # qcreator stuff 29 | CMakeLists.txt.user 30 | 31 | srv/_*.py 32 | *.pcd 33 | *.pyc 34 | qtcreator-* 35 | *.user 36 | 37 | /planning/cfg 38 | /planning/docs 39 | /planning/src 40 | 41 | *~ 42 | 43 | # Emacs 44 | .#* 45 | 46 | # vim 47 | .*.sw[a-z] 48 | *.un~ 49 | Session.vim 50 | .netrwhist 51 | 52 | # Catkin custom files 53 | CATKIN_IGNORE 54 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # This config file for Travis CI utilizes ros-industrial/industrial_ci package. 2 | # For more info for the package, see https://github.com/ros-industrial/industrial_ci/blob/master/README.rst 3 | sudo: required 4 | dist: trusty 5 | services: 6 | - docker 7 | language: generic 8 | compiler: 9 | - gcc 10 | notifications: 11 | email: 12 | on_success: change 13 | on_failue: always 14 | slack: cir-kit:8iPeoxDoStM1laVja019fQAN 15 | env: 16 | global: 17 | - CATKIN_PARALLEL_TEST_JOBS=-p1 18 | - ROS_PARALLEL_TEST_JOBS=-j1 19 | matrix: 20 | - ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu 21 | - ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu 22 | # comment out this line because a log of the following exceeds the limit of log lise 4GB 23 | # - ROS_DISTRO="kinetic" UPSTREAM_WORKSPACE=https://raw.githubusercontent.com/ros-controls/ros_control/$ROS_DISTRO-devel/ros_control.rosinstall 24 | install: 25 | - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config 26 | script: 27 | - source .ci_config/travis.sh 28 | ## - source ./travis.sh # Enable this when you have a package-local script 29 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2016 CIR-KIT 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # steer_drive_ros [![Build Status](https://travis-ci.org/CIR-KIT/steer_drive_ros.svg?branch=kinetic-devel)](https://travis-ci.org/CIR-KIT/steer_drive_ros) [![Slack](https://img.shields.io/badge/Slack-CIR--KIT-blue.svg)](http://cir-kit.slack.com/messages/steer_drive_ros) 2 | Steer drive package for ROS 3 | 4 | 5 | 6 | | Package | Indigo | Jade | Kinetic | Lunar | 7 | |:-------:|:------:|:----:|:-------:|:-----:| 8 | | Status | [![Build Status](https://travis-ci.org/CIR-KIT/steer_drive_ros.svg?branch=indigo-devel)](https://travis-ci.org/CIR-KIT/steer_drive_ros) | [![Build Status](https://travis-ci.org/CIR-KIT/steer_drive_ros.svg?branch=jade-devel)](https://travis-ci.org/CIR-KIT/steer_drive_ros) | [![Build Status](https://travis-ci.org/CIR-KIT/steer_drive_ros.svg?branch=kinetic-devel)](https://travis-ci.org/CIR-KIT/steer_drive_ros) | [![Build Status](https://travis-ci.org/CIR-KIT/steer_drive_ros.svg?branch=lunar-devel)](https://travis-ci.org/CIR-KIT/steer_drive_ros) | 9 | 10 | See [steer_drive_ros documentation](http://wiki.ros.org/steer_drive_ros) on ros.org 11 | -------------------------------------------------------------------------------- /steer_bot_hardware_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(steer_bot_hardware_gazebo) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | find_package(catkin REQUIRED 7 | COMPONENTS 8 | roscpp 9 | angles 10 | control_toolbox 11 | gazebo_ros_control 12 | hardware_interface 13 | joint_limits_interface 14 | ) 15 | 16 | find_package(gazebo REQUIRED) 17 | 18 | catkin_package( 19 | INCLUDE_DIRS include 20 | LIBRARIES ${PROJECT_NAME} 21 | CATKIN_DEPENDS 22 | roscpp 23 | angles 24 | control_toolbox 25 | gazebo_ros_control 26 | hardware_interface 27 | joint_limits_interface 28 | ) 29 | 30 | include_directories(include ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS}) 31 | 32 | add_library(${PROJECT_NAME} src/steer_bot_hardware_gazebo.cpp) 33 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) 34 | 35 | install(TARGETS ${PROJECT_NAME} 36 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 37 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 38 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 39 | ) 40 | 41 | install(DIRECTORY include/ 42 | DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} 43 | FILES_MATCHING PATTERN "*.h" 44 | ) 45 | install (FILES steer_bot_hardware_gazebo_plugins.xml 46 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 47 | -------------------------------------------------------------------------------- /steer_bot_hardware_gazebo/README.md: -------------------------------------------------------------------------------- 1 | ### Parameters 2 | 3 | ```yaml 4 | steer_bot_hardware_gazebo: 5 | # rear wheel frame 6 | rear_wheel : 'base_to_wheel' 7 | # steer frame 8 | front_steer : 'base_to_steer' 9 | # virtual rear frame for Gazebo 10 | virtual_rear_wheels : ['base_to_right_rear_wheel', 'base_to_left_rear_wheel'] 11 | virtual_front_wheels : ['base_to_right_front_wheel', 'base_to_left_front_wheel'] 12 | # virtual steer frame for Gazebo 13 | virtual_front_steers : ['base_to_right_front_steer', 'base_to_left_front_steer'] 14 | 15 | # ackermann link mechanism 16 | # true for ackermann, false for parallel link. default: true 17 | enable_ackermann_link: true 18 | # wheels separatation between the right & the left 19 | wheel_separation_w : 0.5 20 | # wheels separatation between the front & the rear 21 | wheel_separation_h : 0.79 22 | ``` 23 | 24 | ```yaml 25 | gains: 26 | 　# PID Gains for rear wheel velocity control 27 | base_to_right_rear_wheel : {p: 100000.0, d: 10.0, i: 0.50, i_clamp: 3.0} 28 | base_to_left_rear_wheel : {p: 100000.0, d: 10.0, i: 0.50, i_clamp: 3.0} 29 | ``` 30 | 31 | ### In case of inavailable of SetVelocity method 32 | - maybe specification 33 | - http://answers.ros.org/question/118546/setting-joint-velocity-in-gazebo/ 34 | - VELOCITY_PID is one way to solve it 35 | -------------------------------------------------------------------------------- /steer_bot_hardware_gazebo/config/pids.yaml: -------------------------------------------------------------------------------- 1 | gains: 2 | base_to_right_rear_wheel : {p: 100000.0, d: 10.0, i: 0.50, i_clamp: 3.0} 3 | base_to_left_rear_wheel : {p: 100000.0, d: 10.0, i: 0.50, i_clamp: 3.0} 4 | 5 | -------------------------------------------------------------------------------- /steer_bot_hardware_gazebo/config/steer_bot_hardware_gazebo.yaml: -------------------------------------------------------------------------------- 1 | steer_bot_hardware_gazebo: 2 | rear_wheel : 'rear_wheel_joint' 3 | front_steer : 'front_steer_joint' 4 | virtual_rear_wheels : ['base_to_right_rear_wheel', 'base_to_left_rear_wheel'] 5 | virtual_front_wheels : ['base_to_right_front_wheel', 'base_to_left_front_wheel'] 6 | virtual_front_steers : ['base_to_right_front_steer', 'base_to_left_front_steer'] 7 | 8 | # ackermann link mechanism 9 | enable_ackermann_link: true 10 | wheel_separation_w : 0.5 11 | wheel_separation_h : 0.79 12 | -------------------------------------------------------------------------------- /steer_bot_hardware_gazebo/include/steer_bot_hardware_gazebo/steer_bot_hardware_gazebo.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef STEER_BOT_HARDWARE_GAZEBO_H 3 | #define STEER_BOT_HARDWARE_GAZEBO_H 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | enum FRONT_REAR 22 | { 23 | FRONT = 0, REAR = 1, 24 | }; 25 | 26 | namespace steer_bot_hardware_gazebo 27 | { 28 | 29 | double clamp(const double val, const double min_val, const double max_val) 30 | { 31 | return std::min(std::max(val, min_val), max_val); 32 | } 33 | 34 | class SteerBotHardwareGazebo : public gazebo_ros_control::RobotHWSim 35 | { 36 | public: 37 | 38 | SteerBotHardwareGazebo(); 39 | 40 | bool initSim(const std::string& robot_namespace, 41 | ros::NodeHandle model_nh, 42 | gazebo::physics::ModelPtr parent_model, 43 | const urdf::Model* const urdf_model, 44 | std::vector transmissions); 45 | 46 | void readSim(ros::Time time, ros::Duration period); 47 | 48 | void writeSim(ros::Time time, ros::Duration period); 49 | 50 | private: 51 | void CleanUp(); 52 | 53 | void GetJointNames(ros::NodeHandle &_nh); 54 | void GetWheelJointNames(ros::NodeHandle &_nh); 55 | void GetSteerJointNames(ros::NodeHandle &_nh); 56 | 57 | void RegisterHardwareInterfaces(); 58 | void RegisterWheelInterface(); 59 | void RegisterSteerInterface(); 60 | void RegisterInterfaceHandles( 61 | hardware_interface::JointStateInterface& _jnt_state_interface, 62 | hardware_interface::JointCommandInterface& _jnt_cmd_interface, 63 | const std::string _jnt_name, double& _jnt_pos, double& _jnt_vel, double& _jnt_eff, double& _jnt_cmd); 64 | void RegisterInterfaceHandles( 65 | hardware_interface::JointStateInterface& _jnt_state_interface, 66 | const std::string _jnt_name, double& _jnt_pos, double& _jnt_vel, double& _jnt_eff); 67 | void RegisterJointStateInterfaceHandle( 68 | hardware_interface::JointStateInterface& _jnt_state_interface, 69 | const std::string _jnt_name, double& _jnt_pos, double& _jnt_vel, double& _jnt_eff); 70 | void RegisterCommandJointInterfaceHandle( 71 | hardware_interface::JointStateInterface& _jnt_state_interface, 72 | hardware_interface::JointCommandInterface& _jnt_cmd_interface, 73 | const std::string _jnt_name, double& _jnt_cmd); 74 | double ComputeEffCommandFromVelError(const int _index, const int _front_rear, ros::Duration _period); 75 | 76 | void GetCurrentState(std::vector& _jnt_pos, std::vector& _jnt_vel, std::vector& _jnt_eff, 77 | const int _if_index, const int _sim_jnt_index); 78 | 79 | private: 80 | // constant 81 | enum { 82 | INDEX_RIGHT = 0, 83 | INDEX_LEFT = 1 84 | }; 85 | // Raw data 86 | unsigned int n_dof_; 87 | std::vector sim_joints_; 88 | 89 | // Joint limits interface 90 | joint_limits_interface::PositionJointSoftLimitsInterface jnt_limits_interface_; 91 | 92 | // PID controllers 93 | std::vector pids_; 94 | 95 | // new added 96 | ros::NodeHandle nh_; 97 | std::string ns_; 98 | 99 | // common 100 | hardware_interface::JointStateInterface jnt_state_interface_; 101 | // 102 | // rear wheel 103 | //-- actual joint(single actuator) 104 | //---- joint name 105 | std::string rear_wheel_jnt_name_; 106 | //---- joint interface parameters 107 | double rear_wheel_jnt_pos_; 108 | double rear_wheel_jnt_vel_; 109 | double rear_wheel_jnt_eff_; 110 | //---- joint interface command 111 | double rear_wheel_jnt_vel_cmd_; 112 | //---- Hardware interface: joint 113 | hardware_interface::VelocityJointInterface rear_wheel_jnt_vel_cmd_interface_; 114 | //hardware_interface::JointStateInterface wheel_jnt_state_interface_; 115 | // 116 | //-- virtual joints(two rear wheels) 117 | //---- joint name 118 | std::vector virtual_rear_wheel_jnt_names_; 119 | //---- joint interface parameters 120 | std::vector virtual_rear_wheel_jnt_pos_; 121 | std::vector virtual_rear_wheel_jnt_vel_; 122 | std::vector virtual_rear_wheel_jnt_eff_; 123 | //---- joint interface command 124 | std::vector virtual_rear_wheel_jnt_vel_cmd_; 125 | //-- virtual joints(two front wheels) 126 | //---- joint name 127 | std::vector virtual_front_wheel_jnt_names_; 128 | //---- joint interface parameters 129 | std::vector virtual_front_wheel_jnt_pos_; 130 | std::vector virtual_front_wheel_jnt_vel_; 131 | std::vector virtual_front_wheel_jnt_eff_; 132 | //---- joint interface command 133 | std::vector virtual_front_wheel_jnt_vel_cmd_; 134 | 135 | // front steer 136 | //-- actual joint(single actuator) 137 | //---- joint name 138 | std::string front_steer_jnt_name_; 139 | //---- joint interface parameters 140 | double front_steer_jnt_pos_; 141 | double front_steer_jnt_vel_; 142 | double front_steer_jnt_eff_; 143 | //---- joint interface command 144 | double front_steer_jnt_pos_cmd_; 145 | //---- Hardware interface: joint 146 | hardware_interface::PositionJointInterface front_steer_jnt_pos_cmd_interface_; 147 | //hardware_interface::JointStateInterface steer_jnt_state_interface_; 148 | // 149 | //-- virtual joints(two steers) 150 | //---- joint name 151 | std::vector virtual_front_steer_jnt_names_; 152 | //---- joint interface parameters 153 | std::vector virtual_front_steer_jnt_pos_; 154 | std::vector virtual_front_steer_jnt_vel_; 155 | std::vector virtual_front_steer_jnt_eff_; 156 | //---- joint interface command 157 | std::vector virtual_front_steer_jnt_pos_cmd_; 158 | //---- Hardware interface: joint 159 | //hardware_interface::VelocityJointInterface front_wheel_jnt_vel_cmd_interface_; 160 | 161 | // ackermann link mechanism 162 | bool enable_ackermann_link_ ; 163 | double wheel_separation_w_; 164 | double wheel_separation_h_; 165 | 166 | int log_cnt_; 167 | 168 | template 169 | std::string containerToString(const T& cont, const std::string& prefix) 170 | { 171 | std::stringstream ss; 172 | ss << prefix; 173 | std::copy(cont.begin(), --cont.end(), std::ostream_iterator(ss, prefix.c_str())); 174 | ss << *(--cont.end()); 175 | return ss.str(); 176 | } 177 | 178 | }; 179 | 180 | } // namespace steer_bot_hardware_gazebo 181 | 182 | #endif // STEER_BOT_HARDWARE_GAZEBO_H 183 | -------------------------------------------------------------------------------- /steer_bot_hardware_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | steer_bot_hardware_gazebo 4 | 0.1.0 5 | steer bot hardware for gazebo simulation 6 | 7 | Masaru Morita 8 | 9 | BSD 10 | 11 | https://github.com/CIR-KIT/steer_drive_ros/issues 12 | https://github.com/CIR-KIT/steer_drive_ros.git 13 | CIR-KIT 14 | Masaru Morita 15 | 16 | catkin 17 | 18 | angles 19 | control_toolbox 20 | gazebo_ros_control 21 | hardware_interface 22 | joint_limits_interface 23 | roscpp 24 | 25 | gazebo 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /steer_bot_hardware_gazebo/path.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # include path を作成する 4 | include_path=$(pwd)/include 5 | 6 | # CPATHに追加 7 | export CPATH=${CPATH}:${include_path}:${gazebo_path}:${sdf_path} 8 | -------------------------------------------------------------------------------- /steer_bot_hardware_gazebo/src/steer_bot_hardware_gazebo.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | #include 12 | 13 | namespace steer_bot_hardware_gazebo 14 | { 15 | using namespace hardware_interface; 16 | 17 | SteerBotHardwareGazebo::SteerBotHardwareGazebo() 18 | : gazebo_ros_control::RobotHWSim() 19 | , ns_("steer_bot_hardware_gazebo/") 20 | , log_cnt_(0) 21 | {} 22 | 23 | 24 | bool SteerBotHardwareGazebo::initSim(const std::string& robot_namespace, 25 | ros::NodeHandle nh, 26 | gazebo::physics::ModelPtr model, 27 | const urdf::Model* const urdf_model, 28 | std::vector transmissions) 29 | { 30 | using gazebo::physics::JointPtr; 31 | 32 | nh_ = nh; 33 | 34 | // Simulation joints 35 | sim_joints_ = model->GetJoints(); 36 | n_dof_ = sim_joints_.size(); 37 | 38 | this->CleanUp(); 39 | this->GetJointNames(nh_); 40 | this->RegisterHardwareInterfaces(); 41 | 42 | nh_.getParam(ns_ + "wheel_separation_w", wheel_separation_w_); 43 | nh_.getParam(ns_ + "wheel_separation_h", wheel_separation_h_); 44 | ROS_INFO_STREAM("wheel_separation_w = " << wheel_separation_w_); 45 | ROS_INFO_STREAM("wheel_separation_h = " << wheel_separation_h_); 46 | 47 | nh_.getParam(ns_ + "enable_ackermann_link", enable_ackermann_link_); 48 | ROS_INFO_STREAM("enable_ackermann_link = " << (enable_ackermann_link_ ? "true" : "false")); 49 | 50 | // Position joint limits interface 51 | std::vector cmd_handle_names = front_steer_jnt_pos_cmd_interface_.getNames(); 52 | for (size_t i = 0; i < cmd_handle_names.size(); ++i) 53 | { 54 | const std::string name = cmd_handle_names[i]; 55 | 56 | // unless current handle is not pos interface for steer, skip 57 | if(name != virtual_front_steer_jnt_names_[INDEX_RIGHT] && name != virtual_front_steer_jnt_names_[INDEX_LEFT]) 58 | continue; 59 | 60 | JointHandle cmd_handle = front_steer_jnt_pos_cmd_interface_.getHandle(name); 61 | 62 | using namespace joint_limits_interface; 63 | boost::shared_ptr urdf_joint = urdf_model->getJoint(name); 64 | JointLimits limits; 65 | SoftJointLimits soft_limits; 66 | if (!getJointLimits(urdf_joint, limits) || !getSoftJointLimits(urdf_joint, soft_limits)) 67 | { 68 | ROS_WARN_STREAM("Joint limits won't be enforced for joint '" << name << "'."); 69 | } 70 | else 71 | { 72 | jnt_limits_interface_.registerHandle( 73 | PositionJointSoftLimitsHandle(cmd_handle, limits, soft_limits)); 74 | 75 | ROS_DEBUG_STREAM("Joint limits will be enforced for joint '" << name << "'."); 76 | } 77 | } 78 | 79 | // PID controllers for wheel 80 | const int virtual_jnt_cnt_ = virtual_rear_wheel_jnt_names_.size(); 81 | pids_.resize(virtual_jnt_cnt_); 82 | 83 | for (size_t i = 0; i < virtual_jnt_cnt_; ++i) 84 | { 85 | const std::string jnt_name = virtual_rear_wheel_jnt_names_[i]; 86 | const ros::NodeHandle joint_nh(nh, "gains/" + jnt_name); 87 | 88 | ROS_INFO_STREAM("Trying to set pid param of '" << jnt_name << " ' at PID proc in init()"); 89 | if (!pids_[i].init(joint_nh)) 90 | { 91 | ROS_INFO_STREAM("Faied to set pid param of '" << jnt_name << " ' at PID proc in init()"); 92 | return false; 93 | } 94 | ROS_INFO_STREAM("Succeeded to set pid param of '" << jnt_name << " ' at PID proc in init()"); 95 | } 96 | 97 | return true; 98 | } 99 | 100 | void SteerBotHardwareGazebo::readSim(ros::Time time, ros::Duration period) 101 | { 102 | for(int i = 0; i < sim_joints_.size(); i++) 103 | { 104 | std::string gazebo_jnt_name; 105 | gazebo_jnt_name = sim_joints_[i]->GetName(); 106 | 107 | if(gazebo_jnt_name == virtual_rear_wheel_jnt_names_[INDEX_RIGHT]) 108 | { 109 | GetCurrentState(virtual_rear_wheel_jnt_pos_, virtual_rear_wheel_jnt_vel_, virtual_rear_wheel_jnt_eff_, INDEX_RIGHT, i); 110 | } 111 | else if(gazebo_jnt_name == virtual_rear_wheel_jnt_names_[INDEX_LEFT]) 112 | { 113 | GetCurrentState(virtual_rear_wheel_jnt_pos_, virtual_rear_wheel_jnt_vel_, virtual_rear_wheel_jnt_eff_, INDEX_LEFT, i); 114 | } 115 | else if(gazebo_jnt_name == virtual_front_wheel_jnt_names_[INDEX_RIGHT]) 116 | { 117 | GetCurrentState(virtual_front_wheel_jnt_pos_, virtual_front_wheel_jnt_vel_, virtual_front_wheel_jnt_eff_, INDEX_RIGHT, i); 118 | } 119 | else if(gazebo_jnt_name == virtual_front_wheel_jnt_names_[INDEX_LEFT]) 120 | { 121 | GetCurrentState(virtual_front_wheel_jnt_pos_, virtual_front_wheel_jnt_vel_, virtual_front_wheel_jnt_eff_, INDEX_LEFT, i); 122 | } 123 | else if(gazebo_jnt_name == virtual_front_steer_jnt_names_[INDEX_RIGHT]) 124 | { 125 | GetCurrentState(virtual_front_steer_jnt_pos_, virtual_front_steer_jnt_vel_, virtual_front_steer_jnt_eff_, INDEX_RIGHT, i); 126 | } 127 | else if(gazebo_jnt_name == virtual_front_steer_jnt_names_[INDEX_LEFT]) 128 | { 129 | GetCurrentState(virtual_front_steer_jnt_pos_, virtual_front_steer_jnt_vel_, virtual_front_steer_jnt_eff_, INDEX_LEFT, i); 130 | } 131 | else 132 | { 133 | // do nothing 134 | } 135 | } 136 | 137 | front_steer_jnt_pos_ = (virtual_front_steer_jnt_pos_[INDEX_RIGHT] + virtual_front_steer_jnt_pos_[INDEX_LEFT]) / virtual_front_steer_jnt_pos_.size(); 138 | front_steer_jnt_vel_ = (virtual_front_steer_jnt_vel_[INDEX_RIGHT] + virtual_front_steer_jnt_vel_[INDEX_LEFT]) / virtual_front_steer_jnt_vel_.size(); 139 | front_steer_jnt_eff_ = (virtual_front_steer_jnt_eff_[INDEX_RIGHT] + virtual_front_steer_jnt_eff_[INDEX_LEFT]) / virtual_front_steer_jnt_eff_.size(); 140 | } 141 | 142 | void SteerBotHardwareGazebo::writeSim(ros::Time time, ros::Duration period) 143 | { 144 | // Enforce joint limits 145 | jnt_limits_interface_.enforceLimits(period); 146 | 147 | log_cnt_++; 148 | for(int i = 0; i < sim_joints_.size(); i++) 149 | { 150 | std::string gazebo_jnt_name; 151 | gazebo_jnt_name = sim_joints_[i]->GetName(); 152 | 153 | // wheels 154 | if(gazebo_jnt_name == virtual_rear_wheel_jnt_names_[INDEX_RIGHT]) 155 | { 156 | //const double eff_cmd = 2*ComputeEffCommandFromVelError(INDEX_RIGHT, REAR, period); 157 | //sim_joints_[i]->SetForce(0u, eff_cmd); 158 | //if(log_cnt_ % 500 == 0) 159 | //{ 160 | // ROS_DEBUG_STREAM("rear_wheel_jnt_vel_cmd_ = " << rear_wheel_jnt_vel_cmd_); 161 | // ROS_DEBUG_STREAM("virtual_rear_wheel_jnt_vel_[INDEX_RIGHT] = " << virtual_rear_wheel_jnt_vel_[INDEX_RIGHT]); 162 | // ROS_DEBUG_STREAM("error[INDEX_RIGHT] " << rear_wheel_jnt_vel_cmd_ - virtual_rear_wheel_jnt_vel_[INDEX_RIGHT]); 163 | // ROS_DEBUG_STREAM("command[INDEX_RIGHT] = " << eff_cmd); 164 | //} 165 | sim_joints_[i]->SetVelocity(0, rear_wheel_jnt_vel_cmd_); 166 | } 167 | else if(gazebo_jnt_name == virtual_rear_wheel_jnt_names_[INDEX_LEFT]) 168 | { 169 | //const double eff_cmd = 2*ComputeEffCommandFromVelError(INDEX_LEFT, REAR, period); 170 | //sim_joints_[i]->SetForce(0u, eff_cmd); 171 | //if(log_cnt_ % 500 == 0) 172 | //{ 173 | // ROS_DEBUG_STREAM("rear_wheel_jnt_vel_cmd_ = " << rear_wheel_jnt_vel_cmd_); 174 | // ROS_DEBUG_STREAM("virtual_rear_wheel_jnt_vel_[INDEX_LEFT] = " << virtual_rear_wheel_jnt_vel_[INDEX_LEFT]); 175 | // ROS_DEBUG_STREAM("error[INDEX_LEFT] " << rear_wheel_jnt_vel_cmd_ - virtual_rear_wheel_jnt_vel_[INDEX_LEFT]); 176 | // ROS_DEBUG_STREAM("command[INDEX_LEFT] = " << eff_cmd); 177 | //} 178 | sim_joints_[i]->SetVelocity(0, rear_wheel_jnt_vel_cmd_); 179 | } 180 | else if(gazebo_jnt_name == virtual_front_wheel_jnt_names_[INDEX_RIGHT]) 181 | { 182 | //const double eff_cmd = 2*ComputeEffCommandFromVelError(INDEX_RIGHT, FRONT, period); 183 | //sim_joints_[i]->SetForce(0u, eff_cmd); 184 | //ROS_INFO_STREAM("command[INDEX_RIGHT] = " << eff_cmd); 185 | sim_joints_[i]->SetVelocity(0, rear_wheel_jnt_vel_cmd_); // added for gazebo in kinetic 186 | } 187 | else if(gazebo_jnt_name == virtual_front_wheel_jnt_names_[INDEX_LEFT]) 188 | { 189 | //const double eff_cmd = 2*ComputeEffCommandFromVelError(INDEX_LEFT, FRONT, period); 190 | //sim_joints_[i]->SetForce(0u, eff_cmd); 191 | //ROS_INFO_STREAM("command[INDEX_LEFT] = " << eff_cmd); 192 | sim_joints_[i]->SetVelocity(0, rear_wheel_jnt_vel_cmd_); // added for gazebo in kinetic 193 | 194 | } 195 | // steers 196 | else if(gazebo_jnt_name == front_steer_jnt_name_) 197 | { 198 | front_steer_jnt_pos_ = front_steer_jnt_pos_cmd_; 199 | ROS_INFO_STREAM("front_steer_jnt_pos_ '" << front_steer_jnt_pos_ << " ' at writeSim()"); 200 | } 201 | else if(gazebo_jnt_name == virtual_front_steer_jnt_names_[INDEX_RIGHT]) 202 | { 203 | double pos_cmd = 0.0; 204 | if(enable_ackermann_link_) 205 | { 206 | const double h = wheel_separation_h_; 207 | const double w = wheel_separation_w_; 208 | pos_cmd = atan2(2*h*tan(front_steer_jnt_pos_cmd_), 2*h + w/2.0*tan(front_steer_jnt_pos_cmd_)); 209 | ROS_DEBUG_STREAM("ackermann steer angle: " << pos_cmd << " at RIGHT"); 210 | } 211 | else 212 | { 213 | pos_cmd = front_steer_jnt_pos_cmd_; 214 | } 215 | #if GAZEBO_MAJOR_VERSION >= 9 216 | sim_joints_[i]->SetPosition(0, pos_cmd, true); 217 | #else 218 | sim_joints_[i]->SetPosition(0, pos_cmd); 219 | #endif 220 | } 221 | else if(gazebo_jnt_name == virtual_front_steer_jnt_names_[INDEX_LEFT]) 222 | { 223 | double pos_cmd = 0.0; 224 | if(enable_ackermann_link_) 225 | { 226 | const double h = wheel_separation_h_; 227 | const double w = wheel_separation_w_; 228 | pos_cmd = atan2(2*h*tan(front_steer_jnt_pos_cmd_), 2*h - w/2.0*tan(front_steer_jnt_pos_cmd_)); 229 | ROS_DEBUG_STREAM("ackermann steer angle: " << pos_cmd << " at LEFT"); 230 | } 231 | else 232 | { 233 | pos_cmd = 2*front_steer_jnt_pos_cmd_; 234 | } 235 | #if GAZEBO_MAJOR_VERSION >= 9 236 | sim_joints_[i]->SetPosition(0, pos_cmd, true); 237 | #else 238 | sim_joints_[i]->SetPosition(0, pos_cmd); 239 | #endif 240 | } 241 | else 242 | { 243 | // do nothing 244 | } 245 | } 246 | 247 | // steer joint for steer_drive_controller 248 | front_steer_jnt_pos_ = front_steer_jnt_pos_cmd_; 249 | if(log_cnt_ % 500 == 0) 250 | { 251 | ROS_DEBUG_STREAM("front_steer_jnt_pos_ '" << front_steer_jnt_pos_ << " ' at writeSim()"); 252 | } 253 | // wheel joint for steer_drive_controller 254 | rear_wheel_jnt_pos_ = 0.5 * (virtual_rear_wheel_jnt_pos_[INDEX_RIGHT] + virtual_rear_wheel_jnt_pos_[INDEX_LEFT]); 255 | rear_wheel_jnt_vel_ = 0.5 * (virtual_rear_wheel_jnt_vel_[INDEX_RIGHT] + virtual_rear_wheel_jnt_vel_[INDEX_LEFT]); 256 | if(log_cnt_ % 500 == 0) 257 | { 258 | ROS_DEBUG_STREAM("rear_wheel_jnt_pos_ '" << rear_wheel_jnt_pos_ << " ' at writeSim()"); 259 | ROS_DEBUG_STREAM("rear_wheel_jnt_vel_ '" << rear_wheel_jnt_vel_ << " ' at writeSim()"); 260 | } 261 | } 262 | 263 | void SteerBotHardwareGazebo::CleanUp() 264 | { 265 | // wheel 266 | //-- wheel joint names 267 | rear_wheel_jnt_name_.empty(); 268 | virtual_rear_wheel_jnt_names_.clear(); 269 | //-- actual rear wheel joint 270 | rear_wheel_jnt_pos_ = 0; 271 | rear_wheel_jnt_vel_ = 0; 272 | rear_wheel_jnt_eff_ = 0; 273 | rear_wheel_jnt_vel_cmd_ = 0; 274 | //-- virtual rear wheel joint 275 | virtual_rear_wheel_jnt_pos_.clear(); 276 | virtual_rear_wheel_jnt_vel_.clear(); 277 | virtual_rear_wheel_jnt_eff_.clear(); 278 | virtual_rear_wheel_jnt_vel_cmd_.clear(); 279 | //-- virtual front wheel joint 280 | virtual_front_wheel_jnt_pos_.clear(); 281 | virtual_front_wheel_jnt_vel_.clear(); 282 | virtual_front_wheel_jnt_eff_.clear(); 283 | virtual_front_wheel_jnt_vel_cmd_.clear(); 284 | 285 | // steer 286 | //-- steer joint names 287 | front_steer_jnt_name_.empty(); 288 | virtual_front_steer_jnt_names_.clear(); 289 | //-- front steer joint 290 | front_steer_jnt_pos_ = 0; 291 | front_steer_jnt_vel_ = 0; 292 | front_steer_jnt_eff_ = 0; 293 | front_steer_jnt_pos_cmd_ = 0; 294 | //-- virtual wheel joint 295 | virtual_front_steer_jnt_pos_.clear(); 296 | virtual_front_steer_jnt_vel_.clear(); 297 | virtual_front_steer_jnt_eff_.clear(); 298 | virtual_front_steer_jnt_pos_cmd_.clear(); 299 | } 300 | 301 | void SteerBotHardwareGazebo::GetJointNames(ros::NodeHandle &_nh) 302 | { 303 | this->GetWheelJointNames(_nh); 304 | this->GetSteerJointNames(_nh); 305 | } 306 | 307 | void SteerBotHardwareGazebo::GetWheelJointNames(ros::NodeHandle &_nh) 308 | { 309 | // wheel joint to get linear command 310 | _nh.getParam(ns_ + "rear_wheel", rear_wheel_jnt_name_); 311 | 312 | // virtual wheel joint for gazebo control 313 | _nh.getParam(ns_ + "virtual_rear_wheels", virtual_rear_wheel_jnt_names_); 314 | int dof = virtual_rear_wheel_jnt_names_.size(); 315 | virtual_rear_wheel_jnt_pos_.resize(dof); 316 | virtual_rear_wheel_jnt_vel_.resize(dof); 317 | virtual_rear_wheel_jnt_eff_.resize(dof); 318 | virtual_rear_wheel_jnt_vel_cmd_.resize(dof); 319 | 320 | _nh.getParam(ns_ + "virtual_front_wheels", virtual_front_wheel_jnt_names_); 321 | dof = virtual_front_wheel_jnt_names_.size(); 322 | virtual_front_wheel_jnt_pos_.resize(dof); 323 | virtual_front_wheel_jnt_vel_.resize(dof); 324 | virtual_front_wheel_jnt_eff_.resize(dof); 325 | virtual_front_wheel_jnt_vel_cmd_.resize(dof); 326 | 327 | } 328 | 329 | void SteerBotHardwareGazebo::GetSteerJointNames(ros::NodeHandle &_nh) 330 | { 331 | // steer joint to get angular command 332 | _nh.getParam(ns_ + "front_steer", front_steer_jnt_name_); 333 | 334 | // virtual steer joint for gazebo control 335 | _nh.getParam(ns_ + "virtual_front_steers", virtual_front_steer_jnt_names_); 336 | 337 | const int dof = virtual_front_steer_jnt_names_.size(); 338 | virtual_front_steer_jnt_pos_.resize(dof); 339 | virtual_front_steer_jnt_vel_.resize(dof); 340 | virtual_front_steer_jnt_eff_.resize(dof); 341 | virtual_front_steer_jnt_pos_cmd_.resize(dof); 342 | } 343 | 344 | void SteerBotHardwareGazebo::RegisterHardwareInterfaces() 345 | { 346 | this->RegisterSteerInterface(); 347 | this->RegisterWheelInterface(); 348 | 349 | // register mapped interface to ros_control 350 | registerInterface(&jnt_state_interface_); 351 | registerInterface(&rear_wheel_jnt_vel_cmd_interface_); 352 | registerInterface(&front_steer_jnt_pos_cmd_interface_); 353 | } 354 | 355 | void SteerBotHardwareGazebo::RegisterInterfaceHandles( 356 | hardware_interface::JointStateInterface& _jnt_state_interface, 357 | hardware_interface::JointCommandInterface& _jnt_cmd_interface, 358 | const std::string _jnt_name, double& _jnt_pos, double& _jnt_vel, double& _jnt_eff, double& _jnt_cmd) 359 | { 360 | // register joint (both JointState and CommandJoint) 361 | this->RegisterJointStateInterfaceHandle(_jnt_state_interface, _jnt_name, 362 | _jnt_pos, _jnt_vel, _jnt_eff); 363 | this->RegisterCommandJointInterfaceHandle(_jnt_state_interface, _jnt_cmd_interface, 364 | _jnt_name, _jnt_cmd); 365 | } 366 | 367 | void SteerBotHardwareGazebo::RegisterInterfaceHandles( 368 | hardware_interface::JointStateInterface& _jnt_state_interface, 369 | const std::string _jnt_name, double& _jnt_pos, double& _jnt_vel, double& _jnt_eff) 370 | { 371 | // register joint (both JointState and CommandJoint) 372 | this->RegisterJointStateInterfaceHandle(_jnt_state_interface, _jnt_name, 373 | _jnt_pos, _jnt_vel, _jnt_eff); 374 | } 375 | 376 | void SteerBotHardwareGazebo::RegisterWheelInterface() 377 | { 378 | // actual wheel joints 379 | this->RegisterInterfaceHandles( 380 | jnt_state_interface_, rear_wheel_jnt_vel_cmd_interface_, 381 | rear_wheel_jnt_name_, rear_wheel_jnt_pos_, rear_wheel_jnt_vel_, rear_wheel_jnt_eff_, rear_wheel_jnt_vel_cmd_); 382 | 383 | // virtual rear wheel joints 384 | for (int i = 0; i < virtual_rear_wheel_jnt_names_.size(); i++) 385 | { 386 | this->RegisterInterfaceHandles( 387 | jnt_state_interface_, rear_wheel_jnt_vel_cmd_interface_, 388 | virtual_rear_wheel_jnt_names_[i], virtual_rear_wheel_jnt_pos_[i], virtual_rear_wheel_jnt_vel_[i], virtual_rear_wheel_jnt_eff_[i], virtual_rear_wheel_jnt_vel_cmd_[i]); 389 | } 390 | // virtual front wheel joints 391 | for (int i = 0; i < virtual_front_wheel_jnt_names_.size(); i++) 392 | { 393 | this->RegisterInterfaceHandles( 394 | jnt_state_interface_, //front_wheel_jnt_vel_cmd_interface_, 395 | virtual_front_wheel_jnt_names_[i], virtual_front_wheel_jnt_pos_[i], virtual_front_wheel_jnt_vel_[i], virtual_front_wheel_jnt_eff_[i]/*, virtual_front_wheel_jnt_vel_cmd_[i]*/); 396 | } 397 | } 398 | 399 | void SteerBotHardwareGazebo::RegisterSteerInterface() 400 | { 401 | // actual steer joints 402 | this->RegisterInterfaceHandles( 403 | jnt_state_interface_, front_steer_jnt_pos_cmd_interface_, 404 | front_steer_jnt_name_, front_steer_jnt_pos_, front_steer_jnt_vel_, front_steer_jnt_eff_, front_steer_jnt_pos_cmd_); 405 | 406 | // virtual steer joints 407 | for (int i = 0; i < virtual_front_steer_jnt_names_.size(); i++) 408 | { 409 | this->RegisterInterfaceHandles( 410 | jnt_state_interface_, front_steer_jnt_pos_cmd_interface_, 411 | virtual_front_steer_jnt_names_[i], virtual_front_steer_jnt_pos_[i], virtual_front_steer_jnt_vel_[i], virtual_front_steer_jnt_eff_[i], virtual_front_steer_jnt_pos_cmd_[i]); 412 | } 413 | } 414 | 415 | void SteerBotHardwareGazebo::RegisterJointStateInterfaceHandle( 416 | hardware_interface::JointStateInterface& _jnt_state_interface, 417 | const std::string _jnt_name, double& _jnt_pos, double& _jnt_vel, double& _jnt_eff) 418 | { 419 | hardware_interface::JointStateHandle state_handle(_jnt_name, 420 | &_jnt_pos, 421 | &_jnt_vel, 422 | &_jnt_eff); 423 | _jnt_state_interface.registerHandle(state_handle); 424 | 425 | ROS_INFO_STREAM("Registered joint '" << _jnt_name << " ' in the JointStateInterface"); 426 | } 427 | 428 | void SteerBotHardwareGazebo::RegisterCommandJointInterfaceHandle( 429 | hardware_interface::JointStateInterface& _jnt_state_interface, 430 | hardware_interface::JointCommandInterface& _jnt_cmd_interface, 431 | const std::string _jnt_name, double& _jnt_cmd) 432 | { 433 | // joint command 434 | hardware_interface::JointHandle _handle(_jnt_state_interface.getHandle(_jnt_name), 435 | &_jnt_cmd); 436 | _jnt_cmd_interface.registerHandle(_handle); 437 | 438 | ROS_INFO_STREAM("Registered joint '" << _jnt_name << " ' in the CommandJointInterface"); 439 | } 440 | 441 | 442 | double SteerBotHardwareGazebo::ComputeEffCommandFromVelError( 443 | const int _index, const int _front_rear, ros::Duration _period) 444 | { 445 | double vel_error = 0; 446 | if(_front_rear == FRONT) 447 | vel_error = rear_wheel_jnt_vel_cmd_ - virtual_front_wheel_jnt_vel_[_index]; 448 | else if(_front_rear == REAR) 449 | vel_error = rear_wheel_jnt_vel_cmd_ - virtual_rear_wheel_jnt_vel_[_index]; 450 | 451 | ROS_DEBUG_STREAM("vel_error = " << vel_error); 452 | if(fabs(vel_error) < 0.1) 453 | { 454 | vel_error = 0.0; 455 | ROS_DEBUG_STREAM("too small. vel_error <- 0"); 456 | } 457 | else 458 | ROS_DEBUG_STREAM("not small. "); 459 | 460 | const double command = pids_[_index].computeCommand(vel_error, _period); 461 | ROS_DEBUG_STREAM("command =" << command); 462 | 463 | const double effort_limit = 10.0; 464 | const double effort = clamp(command, 465 | -effort_limit, effort_limit); 466 | return effort; 467 | } 468 | 469 | void SteerBotHardwareGazebo::GetCurrentState(std::vector& _jnt_pos, std::vector& _jnt_vel, std::vector& _jnt_eff, 470 | const int _if_index, const int _sim_jnt_index) 471 | { 472 | #if GAZEBO_MAJOR_VERSION >= 9 473 | _jnt_pos[_if_index] += 474 | angles::shortest_angular_distance(_jnt_pos[_if_index], sim_joints_[_sim_jnt_index]->Position(0u)); 475 | #else 476 | _jnt_pos[_if_index] += 477 | angles::shortest_angular_distance(_jnt_pos[_if_index], sim_joints_[_sim_jnt_index]->GetAngle(0u).Radian()); 478 | #endif 479 | _jnt_vel[_if_index] = sim_joints_[_sim_jnt_index]->GetVelocity(0u); 480 | _jnt_eff[_if_index] = sim_joints_[_sim_jnt_index]->GetForce(0u); 481 | } 482 | 483 | } // namespace rosbook_hardware_gazebo 484 | 485 | PLUGINLIB_EXPORT_CLASS(steer_bot_hardware_gazebo::SteerBotHardwareGazebo, gazebo_ros_control::RobotHWSim) 486 | -------------------------------------------------------------------------------- /steer_bot_hardware_gazebo/steer_bot_hardware_gazebo_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | Steer Book arm simulation interface. 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /steer_drive_controller/.fig/rocker_bogie_left.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CIR-KIT/steer_drive_ros/b4faca4108ce401c2ee88fee92b5e93172d8fecb/steer_drive_controller/.fig/rocker_bogie_left.png -------------------------------------------------------------------------------- /steer_drive_controller/.fig/rocker_bogie_right.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CIR-KIT/steer_drive_ros/b4faca4108ce401c2ee88fee92b5e93172d8fecb/steer_drive_controller/.fig/rocker_bogie_right.png -------------------------------------------------------------------------------- /steer_drive_controller/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package diff_drive_controller 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.9.3 (2016-02-12) 6 | ------------------ 7 | * Reduced pedantry, redundancy. 8 | * Added tests for the odom_frame_id parameter. 9 | * Parameterized diff_drive_controller's odom_frame_id 10 | * add check for multiple publishers on cmd_vel 11 | * Address -Wunused-parameter warnings 12 | * Limit jerk 13 | * Add param velocity_rolling_window_size 14 | * Minor fixes 15 | 1. Coding style 16 | 2. Tolerance to fall-back to Runge-Kutta 2 integration 17 | 3. Remove unused variables 18 | * Fix forward test 19 | Fix the following bugs in the testForward test: 20 | 1. Check traveled distance in XY plane 21 | 2. Use expected speed variable on test check 22 | * Add test for NaN 23 | * Add test for bad URDF 24 | This unit test exercises a controller load failure caused by 25 | a wrong wheel geometry. The controller requires that wheels be 26 | modeled by cylinders, while the bad URDF uses spheres. 27 | * Contributors: Adolfo Rodriguez Tsouroukdissian, Bence Magyar, Enrique Fernandez, Eric Tappan, Karsten Knese, Paul Mathieu, tappan-at-git 28 | 29 | 0.9.2 (2015-05-04) 30 | ------------------ 31 | * Allow the wheel separation and radius to be set from different sources 32 | i.e. one can be set from the URDF, the other from the parameter server. 33 | If wheel separation and wheel diameter is specified in the parameter server, don't look them up from urdf 34 | * Contributors: Bence Magyar, Nils Berg 35 | 36 | 0.9.1 (2014-11-03) 37 | ------------------ 38 | 39 | 0.9.0 (2014-10-31) 40 | ------------------ 41 | * Add support for multiple wheels per side 42 | * Odometry computation: 43 | - New option to compute in open loop fashion 44 | - New option to skip publishing odom frame to tf 45 | * Remove dependency on angles package 46 | * Buildsystem fixes 47 | * Contributors: Bence Magyar, Lukas Bulwahn, efernandez 48 | 49 | 0.8.1 (2014-07-11) 50 | ------------------ 51 | 52 | 0.8.0 (2014-05-12) 53 | ------------------ 54 | * Add base_frame_id param (defaults to base_link) 55 | The nav_msgs/Odometry message specifies the child_frame_id field, 56 | which was previously not set. 57 | This commit creates a parameter to replace the previously hard-coded 58 | value of the child_frame_id of the published tf frame, and uses it 59 | in the odom message as well. 60 | * Contributors: enriquefernandez 61 | 62 | 0.7.2 (2014-04-01) 63 | ------------------ 64 | 65 | 0.7.1 (2014-03-31) 66 | ------------------ 67 | * Changed test-depend to build-depend for release jobs. 68 | * Contributors: Bence Magyar 69 | 70 | 0.7.0 (2014-03-28) 71 | ------------------ 72 | * diff_drive_controller: New controller for differential drive wheel systems. 73 | * Control is in the form of a velocity command, that is split then sent on the two wheels of a differential drive 74 | wheel base. 75 | * Odometry is published to tf and to a dedicated nav__msgs/Odometry topic. 76 | * Realtime-safe implementation. 77 | * Implements task-space velocity and acceleration limits. 78 | * Automatic stop after command time-out. 79 | * Contributors: Bence Magyar, Paul Mathieu, Enrique Fernandez. 80 | -------------------------------------------------------------------------------- /steer_drive_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(steer_drive_controller) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | controller_interface 6 | urdf 7 | realtime_tools 8 | tf 9 | nav_msgs 10 | roscpp 11 | angles 12 | control_toolbox 13 | gazebo_ros_control 14 | hardware_interface 15 | joint_limits_interface 16 | ) 17 | 18 | find_package(gazebo REQUIRED) 19 | 20 | catkin_package( 21 | INCLUDE_DIRS include 22 | LIBRARIES ${PROJECT_NAME} 23 | CATKIN_DEPENDS 24 | roscpp 25 | angles 26 | control_toolbox 27 | gazebo_ros_control 28 | hardware_interface 29 | joint_limits_interface 30 | ) 31 | 32 | include_directories( 33 | include ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} 34 | ) 35 | 36 | link_directories(${GAZEBO_LIBRARY_DIRS}) 37 | list(APPEND CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") 38 | 39 | add_library(${PROJECT_NAME} src/steer_drive_controller.cpp src/odometry.cpp src/speed_limiter.cpp) 40 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) 41 | 42 | install(TARGETS ${PROJECT_NAME} 43 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 44 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 45 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 46 | ) 47 | 48 | install(FILES steer_drive_controller_plugins.xml 49 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 50 | 51 | if (CATKIN_ENABLE_TESTING) 52 | find_package(catkin REQUIRED COMPONENTS 53 | rostest 54 | std_srvs 55 | controller_manager 56 | tf 57 | ) 58 | include_directories(include ${catkin_INCLUDE_DIRS}) 59 | 60 | add_executable(${PROJECT_NAME}_steerbot test/common/src/steerbot.cpp) 61 | target_link_libraries(${PROJECT_NAME}_steerbot ${catkin_LIBRARIES}) 62 | 63 | add_dependencies(tests ${PROJECT_NAME}_steerbot) 64 | 65 | add_rostest_gtest(${PROJECT_NAME}_test 66 | test/steer_drive_controller_test/steer_drive_controller.test 67 | test/steer_drive_controller_test/steer_drive_controller_test.cpp) 68 | target_link_libraries(${PROJECT_NAME}_test ${catkin_LIBRARIES}) 69 | add_rostest_gtest(${PROJECT_NAME}_nan_test 70 | test/steer_drive_controller_nan_test/steer_drive_controller_nan.test 71 | test/steer_drive_controller_nan_test/steer_drive_controller_nan_test.cpp) 72 | target_link_libraries(${PROJECT_NAME}_nan_test ${catkin_LIBRARIES}) 73 | add_rostest_gtest(${PROJECT_NAME}_limits_test 74 | test/steer_drive_controller_limits_test/steer_drive_controller_limits.test 75 | test/steer_drive_controller_limits_test/steer_drive_controller_limits_test.cpp) 76 | target_link_libraries(${PROJECT_NAME}_limits_test ${catkin_LIBRARIES}) 77 | add_rostest_gtest(${PROJECT_NAME}_timeout_test 78 | test/steer_drive_controller_timeout_test/steer_drive_controller_timeout.test 79 | test/steer_drive_controller_timeout_test/steer_drive_controller_timeout_test.cpp) 80 | target_link_libraries(${PROJECT_NAME}_timeout_test ${catkin_LIBRARIES}) 81 | add_rostest_gtest(steer_drive_controller_fail_test 82 | test/steer_drive_controller_fail_test/steer_drive_controller_wrong.test 83 | test/steer_drive_controller_fail_test/steer_drive_controller_fail_test.cpp) 84 | target_link_libraries(steer_drive_controller_fail_test ${catkin_LIBRARIES}) 85 | add_rostest_gtest(${PROJECT_NAME}_odom_tf_test 86 | test/steer_drive_controller_odom_tf_test/steer_drive_controller_odom_tf.test 87 | test/steer_drive_controller_odom_tf_test/steer_drive_controller_odom_tf_test.cpp) 88 | target_link_libraries(${PROJECT_NAME}_odom_tf_test ${catkin_LIBRARIES}) 89 | add_rostest_gtest(${PROJECT_NAME}_default_odom_frame_test 90 | test/steer_drive_controller_default_odom_frame_test/steer_drive_controller_default_odom_frame.test 91 | test/steer_drive_controller_default_odom_frame_test/steer_drive_controller_default_odom_frame_test.cpp) 92 | target_link_libraries(${PROJECT_NAME}_default_odom_frame_test ${catkin_LIBRARIES}) 93 | add_rostest_gtest(steer_drive_controller_odom_frame_test 94 | test/steer_drive_controller_odom_frame_test/steer_drive_controller_odom_frame.test 95 | test/steer_drive_controller_odom_frame_test/steer_drive_controller_odom_frame_test.cpp) 96 | target_link_libraries(${PROJECT_NAME}_odom_frame_test ${catkin_LIBRARIES}) 97 | add_rostest(test/steer_drive_controller_open_loop_test/steer_drive_controller_open_loop.test) 98 | add_rostest(test/steer_drive_controller_no_wheel_test/steer_drive_controller_no_wheel.test) 99 | add_rostest(test/steer_drive_controller_no_steer_test/steer_drive_controller_no_steer.test) 100 | add_rostest(test/steer_drive_controller_radius_param_test/steer_drive_controller_radius_param.test) 101 | add_rostest(test/steer_drive_controller_radius_param_fail_test/steer_drive_controller_radius_param_fail.test) 102 | add_rostest(test/steer_drive_controller_separation_param_test/steer_drive_controller_separation_param.test) 103 | endif() 104 | -------------------------------------------------------------------------------- /steer_drive_controller/README.md: -------------------------------------------------------------------------------- 1 | ## Steer Drive Controller ## 2 | 3 | Controller for a steer drive mobile base. 4 | 5 | ## 仕様 6 | - Subscribe 7 | - `steer_drive_controller/cmd_vel` にTwist型のメッセージを投げて下さい． 8 | - Publish 9 | - `steer_drive_controller/odom` を出すべきなのですが，未完成です． 10 | 11 | - 例：SteerDriveController + SteerBot(RobotHWSim) で起動して，以下のメッセージで動く 12 | ```bash 13 | rostopic pub -1 steer_drive_controller/cmd_vel geometry_msgs/Twist -- '[1.0, 0.0, 0.0]' '[0.0, 0.0, 0.5]' 14 | ``` 15 | 16 | ## third_robotのlinkメモ 17 | ### ベース 18 | - base_link 19 | 20 | ### ステアリング 21 | - steer 22 | 23 | ### ホイール 24 | - left_front_wheel 25 | - left_rear_wheel 26 | - right_front_wheel 27 | - right_rear_wheel 28 | 29 | ### センサ 30 | - camera_link 31 | - front_bottom_lrf 32 | - front_top_lrf 33 | - rear_bottom_lrf 34 | 35 | ## third_robotのjointメモ 36 | ### ホイール 37 | - base_to_left_front_wheel 38 | - base_to_left_rear_wheel 39 | - base_to_right_front_wheel 40 | - base_to_right_rear_wheel 41 | 42 | ### ステアリング 43 | - base_to_steer 44 | - base_to_steer_right: gazebo用のvirtual. 45 | - base_to_steer_left: gazebo用のvirtual. 46 | 47 | ## デバッグ開発の手順 48 | ### 準備 49 | - パスを通す 50 | 51 | ```bash 52 | source path.bash 53 | ``` 54 | 55 | - gazeboを実行する 56 | ```bash 57 | roslaunch third_robot_gazebo third_robot_world.launch 58 | ``` 59 | 60 | - コントローラ動作用の最低限の設定を行うlaunchファイルを実行する 61 | ```bash 62 | roslaunch steer_drive_controller steer_drive_test_setting.launch 63 | ``` 64 | 65 | ### デバッガ 66 | - パスを通したコンソールでQtCreatorを起動する 67 | ```bash 68 | qtcreator 69 | ``` 70 | 71 | - [QtCreatorでROSのパッケージをビルド&デバッグ実行する](http://qiita.com/MoriKen/items/ea41e485929e0724d15e)にしたがってビルドする．実行オプションで`steer_drive_test`を選択する． 72 | 73 | - `third_robot_control/steer_drive_controller/include/steer_drive_controller.h`内で`#define GUI_DEBUG`をアンコメントアウトする． 74 | - 53行目：`#define GUI_DEBUG // uncommentout when you use qtcreator for debugging` ってとこ． 75 | - デバッグ用のモジュールに名前空間がついてないので，無理やり付与する設定． 76 | 77 | - `third_robot_control/steer_drive_controller/test/steer_drive_test.cpp` のmain関数内でブレイクポイントを置いてデバッグ実行． 78 | - SteerRobotはフェイクで作ってある． 79 | - `bool rt_code = rb_controller.init(steer_drive_bot, nh_main, nh_control);` でコントローラの初期化 80 | 81 | -------------------------------------------------------------------------------- /steer_drive_controller/include/steer_drive_controller/odometry.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2013, PAL Robotics, S.L. 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 PAL Robotics 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 | /* 36 | * Author: Luca Marchionni 37 | * Author: Bence Magyar 38 | * Author: Enrique Fernández 39 | * Author: Paul Mathieu 40 | */ 41 | 42 | #ifndef ODOMETRY_H_ 43 | #define ODOMETRY_H_ 44 | 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | namespace steer_drive_controller 52 | { 53 | namespace bacc = boost::accumulators; 54 | 55 | /** 56 | * \brief The Odometry class handles odometry readings 57 | * (2D pose and velocity with related timestamp) 58 | */ 59 | class Odometry 60 | { 61 | public: 62 | 63 | /// Integration function, used to integrate the odometry: 64 | typedef boost::function IntegrationFunction; 65 | 66 | /** 67 | * \brief Constructor 68 | * Timestamp will get the current time value 69 | * Value will be set to zero 70 | * \param velocity_rolling_window_size Rolling window size used to compute the velocity mean 71 | */ 72 | Odometry(size_t velocity_rolling_window_size = 10); 73 | 74 | /** 75 | * \brief Initialize the odometry 76 | * \param time Current time 77 | */ 78 | void init(const ros::Time &time); 79 | 80 | /** 81 | * \brief Updates the odometry class with latest wheels position 82 | * \param rear_wheel_pos Rear wheel position [rad] 83 | * \param front_steer_pos Front Steer position [rad] 84 | * \param time Current time 85 | * \return true if the odometry is actually updated 86 | */ 87 | bool update(double rear_wheel_pos, double front_steer_pos, const ros::Time &time); 88 | 89 | /** 90 | * \brief Updates the odometry class with latest velocity command 91 | * \param linear Linear velocity [m/s] 92 | * \param angular Angular velocity [rad/s] 93 | * \param time Current time 94 | */ 95 | void updateOpenLoop(double linear, double angular, const ros::Time &time); 96 | 97 | /** 98 | * \brief heading getter 99 | * \return heading [rad] 100 | */ 101 | double getHeading() const 102 | { 103 | return heading_; 104 | } 105 | 106 | /** 107 | * \brief x position getter 108 | * \return x position [m] 109 | */ 110 | double getX() const 111 | { 112 | return x_; 113 | } 114 | 115 | /** 116 | * \brief y position getter 117 | * \return y position [m] 118 | */ 119 | double getY() const 120 | { 121 | return y_; 122 | } 123 | 124 | /** 125 | * \brief linear velocity getter 126 | * \return linear velocity [m/s] 127 | */ 128 | double getLinear() const 129 | { 130 | return linear_; 131 | } 132 | 133 | /** 134 | * \brief angular velocity getter 135 | * \return angular velocity [rad/s] 136 | */ 137 | double getAngular() const 138 | { 139 | return angular_; 140 | } 141 | 142 | /** 143 | * \brief Sets the wheel parameters: radius and separation 144 | * \param wheel_separation Seperation between left and right wheels [m] 145 | * \param wheel_radius Wheel radius [m] 146 | */ 147 | void setWheelParams(double wheel_reparation_h, double wheel_radius); 148 | 149 | /** 150 | * \brief Velocity rolling window size setter 151 | * \param velocity_rolling_window_size Velocity rolling window size 152 | */ 153 | void setVelocityRollingWindowSize(size_t velocity_rolling_window_size); 154 | 155 | private: 156 | 157 | /// Rolling mean accumulator and window: 158 | typedef bacc::accumulator_set > RollingMeanAcc; 159 | typedef bacc::tag::rolling_window RollingWindow; 160 | 161 | /** 162 | * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta 163 | * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by encoders 164 | * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders 165 | */ 166 | void integrateRungeKutta2(double linear, double angular); 167 | 168 | /** 169 | * \brief Integrates the velocities (linear and angular) using exact method 170 | * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by encoders 171 | * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders 172 | */ 173 | void integrateExact(double linear, double angular); 174 | 175 | /** 176 | * \brief Reset linear and angular accumulators 177 | */ 178 | void resetAccumulators(); 179 | 180 | /// Current timestamp: 181 | ros::Time timestamp_; 182 | 183 | /// Current pose: 184 | double x_; // [m] 185 | double y_; // [m] 186 | double heading_; // [rad] 187 | 188 | /// Current velocity: 189 | double linear_; // [m/s] 190 | double angular_; // [rad/s] 191 | 192 | /// Wheel kinematic parameters [m]: 193 | double wheel_separation_h_; 194 | double wheel_radius_; 195 | 196 | /// Previous wheel position/state [rad]: 197 | double rear_wheel_old_pos_; 198 | 199 | /// Rolling mean accumulators for the linar and angular velocities: 200 | size_t velocity_rolling_window_size_; 201 | RollingMeanAcc linear_acc_; 202 | RollingMeanAcc angular_acc_; 203 | 204 | /// Integration funcion, used to integrate the odometry: 205 | IntegrationFunction integrate_fun_; 206 | }; 207 | } 208 | 209 | #endif /* ODOMETRY_H_ */ 210 | -------------------------------------------------------------------------------- /steer_drive_controller/include/steer_drive_controller/speed_limiter.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2013, PAL Robotics, S.L. 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 PAL Robotics 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 | /* 36 | * Author: Enrique Fernández 37 | */ 38 | 39 | #ifndef SPEED_LIMITER_H 40 | #define SPEED_LIMITER_H 41 | 42 | namespace steer_drive_controller 43 | { 44 | 45 | class SpeedLimiter 46 | { 47 | public: 48 | 49 | /** 50 | * \brief Constructor 51 | * \param [in] has_velocity_limits if true, applies velocity limits 52 | * \param [in] has_acceleration_limits if true, applies acceleration limits 53 | * \param [in] has_jerk_limits if true, applies jerk limits 54 | * \param [in] min_velocity Minimum velocity [m/s], usually <= 0 55 | * \param [in] max_velocity Maximum velocity [m/s], usually >= 0 56 | * \param [in] min_acceleration Minimum acceleration [m/s^2], usually <= 0 57 | * \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0 58 | * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0 59 | * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 60 | */ 61 | SpeedLimiter( 62 | bool has_velocity_limits = false, 63 | bool has_acceleration_limits = false, 64 | bool has_jerk_limits = false, 65 | double min_velocity = 0.0, 66 | double max_velocity = 0.0, 67 | double min_acceleration = 0.0, 68 | double max_acceleration = 0.0, 69 | double min_jerk = 0.0, 70 | double max_jerk = 0.0 71 | ); 72 | 73 | /** 74 | * \brief Limit the velocity and acceleration 75 | * \param [in, out] v Velocity [m/s] 76 | * \param [in] v0 Previous velocity to v [m/s] 77 | * \param [in] v1 Previous velocity to v0 [m/s] 78 | * \param [in] dt Time step [s] 79 | * \return Limiting factor (1.0 if none) 80 | */ 81 | double limit(double& v, double v0, double v1, double dt); 82 | 83 | /** 84 | * \brief Limit the velocity 85 | * \param [in, out] v Velocity [m/s] 86 | * \return Limiting factor (1.0 if none) 87 | */ 88 | double limit_velocity(double& v); 89 | 90 | /** 91 | * \brief Limit the acceleration 92 | * \param [in, out] v Velocity [m/s] 93 | * \param [in] v0 Previous velocity [m/s] 94 | * \param [in] dt Time step [s] 95 | * \return Limiting factor (1.0 if none) 96 | */ 97 | double limit_acceleration(double& v, double v0, double dt); 98 | 99 | /** 100 | * \brief Limit the jerk 101 | * \param [in, out] v Velocity [m/s] 102 | * \param [in] v0 Previous velocity to v [m/s] 103 | * \param [in] v1 Previous velocity to v0 [m/s] 104 | * \param [in] dt Time step [s] 105 | * \return Limiting factor (1.0 if none) 106 | * \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control 107 | */ 108 | double limit_jerk(double& v, double v0, double v1, double dt); 109 | 110 | public: 111 | // Enable/Disable velocity/acceleration/jerk limits: 112 | bool has_velocity_limits; 113 | bool has_acceleration_limits; 114 | bool has_jerk_limits; 115 | 116 | // Velocity limits: 117 | double min_velocity; 118 | double max_velocity; 119 | 120 | // Acceleration limits: 121 | double min_acceleration; 122 | double max_acceleration; 123 | 124 | // Jerk limits: 125 | double min_jerk; 126 | double max_jerk; 127 | }; 128 | 129 | } // namespace diff_drive_controller 130 | 131 | #endif // SPEED_LIMITER_H 132 | -------------------------------------------------------------------------------- /steer_drive_controller/include/steer_drive_controller/steer_drive_controller.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2013, PAL Robotics, S.L. 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 PAL Robotics 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 | /* 36 | * Author: Enrique Fernández 37 | */ 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | 47 | #include 48 | #include 49 | 50 | #include 51 | #include 52 | 53 | //#define GUI_DEBUG // uncommentout when you use qtcreator for debugging 54 | 55 | namespace steer_drive_controller{ 56 | 57 | /** 58 | * This class makes some assumptions on the model of the robot: 59 | * - the rotation axes of wheels are collinear 60 | * - the wheels are identical in radius 61 | * Additional assumptions to not duplicate information readily available in the URDF: 62 | * - the wheels have the same parent frame 63 | * - a wheel collision geometry is a cylinder in the urdf 64 | * - a wheel joint frame center's vertical projection on the floor must lie within the contact patch 65 | */ 66 | class SteerDriveController 67 | : public controller_interface::MultiInterfaceController< 68 | hardware_interface::PositionJointInterface, 69 | hardware_interface::VelocityJointInterface> 70 | { 71 | // constants 72 | private: 73 | enum INDX_WHEEL { 74 | INDX_WHEEL_FRNT = 0, 75 | INDX_WHEEL_MID = 1, 76 | INDX_WHEEL_BACK = 2, 77 | }; 78 | enum INDX_STEER { 79 | INDX_STEER_FRNT = 0, 80 | INDX_STEER_BACK = 1, 81 | }; 82 | // constant 83 | enum INDEX_WHEEL { 84 | INDEX_RIGHT = 0, 85 | INDEX_LEFT = 1 86 | }; 87 | public: 88 | SteerDriveController(); 89 | 90 | /** 91 | * \brief Initialize controller 92 | * \param hw Velocity joint interface for the wheels 93 | * \param root_nh Node handle at root namespace 94 | * \param controller_nh Node handle inside the controller namespace 95 | */ 96 | bool init(//hardware_interface::VelocityJointInterface *hw, 97 | hardware_interface::RobotHW* robot_hw, 98 | ros::NodeHandle& root_nh, 99 | ros::NodeHandle &controller_nh); 100 | 101 | /** 102 | * \brief Updates controller, i.e. computes the odometry and sets the new velocity commands 103 | * \param time Current time 104 | * \param period Time since the last called to update 105 | */ 106 | void update(const ros::Time& time, const ros::Duration& period); 107 | 108 | /** 109 | * \brief Starts controller 110 | * \param time Current time 111 | */ 112 | void starting(const ros::Time& time); 113 | 114 | /** 115 | * \brief Stops controller 116 | * \param time Current time 117 | */ 118 | void stopping(const ros::Time& /*time*/); 119 | 120 | private: 121 | std::string name_; 122 | 123 | /// Odometry related: 124 | ros::Duration publish_period_; 125 | ros::Time last_state_publish_time_; 126 | bool open_loop_; 127 | 128 | /// Hardware handles: 129 | hardware_interface::JointHandle rear_wheel_joint_; 130 | hardware_interface::JointHandle front_steer_joint_; 131 | 132 | /// Velocity command related: 133 | struct Commands 134 | { 135 | double lin; 136 | double ang; 137 | ros::Time stamp; 138 | 139 | Commands() : lin(0.0), ang(0.0), stamp(0.0) {} 140 | }; 141 | realtime_tools::RealtimeBuffer command_; 142 | Commands command_struct_; 143 | ros::Subscriber sub_command_; 144 | 145 | /// Odometry related: 146 | boost::shared_ptr > odom_pub_; 147 | boost::shared_ptr > tf_odom_pub_; 148 | Odometry odometry_; 149 | 150 | /// Wheel separation, wrt the midpoint of the wheel width: 151 | double wheel_separation_h_; 152 | 153 | /// Wheel radius (assuming it's the same for the left and right wheels): 154 | double wheel_radius_; 155 | 156 | /// Wheel separation and radius calibration multipliers: 157 | double wheel_separation_h_multiplier_; 158 | double wheel_radius_multiplier_; 159 | double steer_pos_multiplier_; 160 | 161 | /// Timeout to consider cmd_vel commands old: 162 | double cmd_vel_timeout_; 163 | 164 | /// Whether to allow multiple publishers on cmd_vel topic or not: 165 | bool allow_multiple_cmd_vel_publishers_; 166 | 167 | /// Frame to use for the robot base: 168 | std::string base_frame_id_; 169 | 170 | /// Frame to use for odometry and odom tf: 171 | std::string odom_frame_id_; 172 | 173 | /// Whether to publish odometry to tf or not: 174 | bool enable_odom_tf_; 175 | 176 | /// Number of wheel joints: 177 | size_t wheel_joints_size_; 178 | 179 | /// Number of steer joints: 180 | size_t steer_joints_size_; 181 | 182 | /// Speed limiters: 183 | Commands last1_cmd_; 184 | Commands last0_cmd_; 185 | SpeedLimiter limiter_lin_; 186 | SpeedLimiter limiter_ang_; 187 | 188 | // FOR_DEBUG 189 | std::string ns_; 190 | 191 | private: 192 | /** 193 | * \brief Brakes the wheels, i.e. sets the velocity to 0 194 | */ 195 | void brake(); 196 | 197 | /** 198 | * \brief Velocity command callback 199 | * \param command Velocity command message (twist) 200 | */ 201 | void cmdVelCallback(const geometry_msgs::Twist& command); 202 | 203 | /** 204 | * \brief Sets odometry parameters from the URDF, i.e. the wheel radius and separation 205 | * \param root_nh Root node handle 206 | * \param left_wheel_name Name of the left wheel joint 207 | * \param right_wheel_name Name of the right wheel joint 208 | */ 209 | bool setOdomParamsFromUrdf(ros::NodeHandle& root_nh, 210 | const std::string rear_wheel_name, 211 | const std::string front_steer_name, 212 | bool lookup_wheel_separation_h, 213 | bool lookup_wheel_radius); 214 | 215 | /** 216 | * \brief Sets the odometry publishing fields 217 | * \param root_nh Root node handle 218 | * \param controller_nh Node handle inside the controller namespace 219 | */ 220 | void setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh); 221 | 222 | }; 223 | 224 | PLUGINLIB_EXPORT_CLASS(steer_drive_controller::SteerDriveController, controller_interface::ControllerBase) 225 | } // namespace diff_drive_controller 226 | -------------------------------------------------------------------------------- /steer_drive_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | steer_drive_controller 4 | 0.1.0 5 | Controller for a steer drive mobile base. 6 | Masaru Morita 7 | 8 | BSD 9 | 10 | https://github.com/CIR-KIT/steer_drive_ros/issues 11 | https://github.com/CIR-KIT/steer_drive_ros.git 12 | CIR-KIT 13 | Masaru Morita 14 | 15 | catkin 16 | 17 | angles 18 | controller_interface 19 | control_toolbox 20 | gazebo_ros_control 21 | hardware_interface 22 | joint_limits_interface 23 | nav_msgs 24 | realtime_tools 25 | roscpp 26 | tf 27 | urdf 28 | 29 | gazebo 30 | 31 | 32 | rostest 33 | 34 | controller_manager 35 | rostest 36 | rosunit 37 | std_srvs 38 | xacro 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /steer_drive_controller/path.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # include path を作成する 4 | include_path=$(pwd)/include 5 | gazebo_path=/usr/include/gazebo-2.2/ 6 | sdf_path=/usr/include/sdformat-1.4/ 7 | 8 | # CPATHに追加 9 | export CPATH=${CPATH}:${include_path}:${gazebo_path}:${sdf_path} 10 | -------------------------------------------------------------------------------- /steer_drive_controller/src/odometry.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2013, PAL Robotics, S.L. 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 PAL Robotics 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 | /* 36 | * Author: Luca Marchionni 37 | * Author: Bence Magyar 38 | * Author: Enrique Fernández 39 | * Author: Paul Mathieu 40 | */ 41 | 42 | #include 43 | 44 | #include 45 | 46 | namespace steer_drive_controller 47 | { 48 | namespace bacc = boost::accumulators; 49 | 50 | Odometry::Odometry(size_t velocity_rolling_window_size) 51 | : timestamp_(0.0) 52 | , x_(0.0) 53 | , y_(0.0) 54 | , heading_(0.0) 55 | , linear_(0.0) 56 | , angular_(0.0) 57 | , wheel_separation_h_(0.0) 58 | , wheel_radius_(0.0) 59 | , rear_wheel_old_pos_(0.0) 60 | , velocity_rolling_window_size_(velocity_rolling_window_size) 61 | , linear_acc_(RollingWindow::window_size = velocity_rolling_window_size) 62 | , angular_acc_(RollingWindow::window_size = velocity_rolling_window_size) 63 | , integrate_fun_(boost::bind(&Odometry::integrateExact, this, _1, _2)) 64 | { 65 | } 66 | 67 | void Odometry::init(const ros::Time& time) 68 | { 69 | // Reset accumulators and timestamp: 70 | resetAccumulators(); 71 | timestamp_ = time; 72 | } 73 | 74 | bool Odometry::update(double rear_wheel_pos, double front_steer_pos, const ros::Time &time) 75 | { 76 | /// Get current wheel joint positions: 77 | const double rear_wheel_cur_pos = rear_wheel_pos * wheel_radius_; 78 | 79 | /// Estimate velocity of wheels using old and current position: 80 | //const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_; 81 | //const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_; 82 | 83 | const double rear_wheel_est_vel = rear_wheel_cur_pos - rear_wheel_old_pos_; 84 | 85 | /// Update old position with current: 86 | rear_wheel_old_pos_ = rear_wheel_cur_pos; 87 | 88 | /// Compute linear and angular diff: 89 | const double linear = rear_wheel_est_vel;//(right_wheel_est_vel + left_wheel_est_vel) * 0.5; 90 | //const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_w_; 91 | const double angular = tan(front_steer_pos) * linear / wheel_separation_h_; 92 | 93 | /// Integrate odometry: 94 | integrate_fun_(linear, angular); 95 | 96 | /// We cannot estimate the speed with very small time intervals: 97 | const double dt = (time - timestamp_).toSec(); 98 | if (dt < 0.0001) 99 | return false; // Interval too small to integrate with 100 | 101 | timestamp_ = time; 102 | 103 | /// Estimate speeds using a rolling mean to filter them out: 104 | linear_acc_(linear/dt); 105 | angular_acc_(angular/dt); 106 | 107 | linear_ = bacc::rolling_mean(linear_acc_); 108 | angular_ = bacc::rolling_mean(angular_acc_); 109 | 110 | return true; 111 | } 112 | 113 | void Odometry::updateOpenLoop(double linear, double angular, const ros::Time &time) 114 | { 115 | /// Save last linear and angular velocity: 116 | linear_ = linear; 117 | angular_ = angular; 118 | 119 | /// Integrate odometry: 120 | const double dt = (time - timestamp_).toSec(); 121 | timestamp_ = time; 122 | integrate_fun_(linear * dt, angular * dt); 123 | } 124 | 125 | void Odometry::setWheelParams(double wheel_separation_h, double wheel_radius) 126 | { 127 | wheel_separation_h_ = wheel_separation_h; 128 | wheel_radius_ = wheel_radius; 129 | } 130 | 131 | void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size) 132 | { 133 | velocity_rolling_window_size_ = velocity_rolling_window_size; 134 | 135 | resetAccumulators(); 136 | } 137 | 138 | void Odometry::integrateRungeKutta2(double linear, double angular) 139 | { 140 | const double direction = heading_ + angular * 0.5; 141 | 142 | /// Runge-Kutta 2nd order integration: 143 | x_ += linear * cos(direction); 144 | y_ += linear * sin(direction); 145 | heading_ += angular; 146 | } 147 | 148 | /** 149 | * \brief Other possible integration method provided by the class 150 | * \param linear 151 | * \param angular 152 | */ 153 | void Odometry::integrateExact(double linear, double angular) 154 | { 155 | if (fabs(angular) < 1e-6) 156 | integrateRungeKutta2(linear, angular); 157 | else 158 | { 159 | /// Exact integration (should solve problems when angular is zero): 160 | const double heading_old = heading_; 161 | const double r = linear/angular; 162 | heading_ += angular; 163 | x_ += r * (sin(heading_) - sin(heading_old)); 164 | y_ += -r * (cos(heading_) - cos(heading_old)); 165 | } 166 | } 167 | 168 | void Odometry::resetAccumulators() 169 | { 170 | linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); 171 | angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); 172 | } 173 | 174 | } // namespace diff_drive_controller 175 | -------------------------------------------------------------------------------- /steer_drive_controller/src/speed_limiter.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2013, PAL Robotics, S.L. 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 PAL Robotics 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 | /* 36 | * Author: Enrique Fernández 37 | */ 38 | 39 | #include 40 | 41 | #include 42 | 43 | template 44 | T clamp(T x, T min, T max) 45 | { 46 | return std::min(std::max(min, x), max); 47 | } 48 | 49 | namespace steer_drive_controller 50 | { 51 | 52 | SpeedLimiter::SpeedLimiter( 53 | bool has_velocity_limits, 54 | bool has_acceleration_limits, 55 | bool has_jerk_limits, 56 | double min_velocity, 57 | double max_velocity, 58 | double min_acceleration, 59 | double max_acceleration, 60 | double min_jerk, 61 | double max_jerk 62 | ) 63 | : has_velocity_limits(has_velocity_limits) 64 | , has_acceleration_limits(has_acceleration_limits) 65 | , has_jerk_limits(has_jerk_limits) 66 | , min_velocity(min_velocity) 67 | , max_velocity(max_velocity) 68 | , min_acceleration(min_acceleration) 69 | , max_acceleration(max_acceleration) 70 | , min_jerk(min_jerk) 71 | , max_jerk(max_jerk) 72 | { 73 | } 74 | 75 | double SpeedLimiter::limit(double& v, double v0, double v1, double dt) 76 | { 77 | const double tmp = v; 78 | 79 | limit_jerk(v, v0, v1, dt); 80 | limit_acceleration(v, v0, dt); 81 | limit_velocity(v); 82 | 83 | return tmp != 0.0 ? v / tmp : 1.0; 84 | } 85 | 86 | double SpeedLimiter::limit_velocity(double& v) 87 | { 88 | const double tmp = v; 89 | 90 | if (has_velocity_limits) 91 | { 92 | v = clamp(v, min_velocity, max_velocity); 93 | } 94 | 95 | return tmp != 0.0 ? v / tmp : 1.0; 96 | } 97 | 98 | double SpeedLimiter::limit_acceleration(double& v, double v0, double dt) 99 | { 100 | const double tmp = v; 101 | 102 | if (has_acceleration_limits) 103 | { 104 | const double dv_min = min_acceleration * dt; 105 | const double dv_max = max_acceleration * dt; 106 | 107 | const double dv = clamp(v - v0, dv_min, dv_max); 108 | 109 | v = v0 + dv; 110 | } 111 | 112 | return tmp != 0.0 ? v / tmp : 1.0; 113 | } 114 | 115 | double SpeedLimiter::limit_jerk(double& v, double v0, double v1, double dt) 116 | { 117 | const double tmp = v; 118 | 119 | if (has_jerk_limits) 120 | { 121 | const double dv = v - v0; 122 | const double dv0 = v0 - v1; 123 | 124 | const double dt2 = 2. * dt * dt; 125 | 126 | const double da_min = min_jerk * dt2; 127 | const double da_max = max_jerk * dt2; 128 | 129 | const double da = clamp(dv - dv0, da_min, da_max); 130 | 131 | v = v0 + dv0 + da; 132 | } 133 | 134 | return tmp != 0.0 ? v / tmp : 1.0; 135 | } 136 | 137 | } // namespace diff_drive_controller 138 | -------------------------------------------------------------------------------- /steer_drive_controller/steer_drive_controller_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | The SteerDriveController tracks velocity commands. It expects 1 VelocityJointInterface and 1 PositionJointInterfece types of hardware interfaces. 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /steer_drive_controller/test/common/config/steerbot_controllers.yaml: -------------------------------------------------------------------------------- 1 | steerbot_controller: 2 | type: "steer_drive_controller/SteerDriveController" 3 | rear_wheel : 'rear_wheel_joint' 4 | front_steer : 'front_steer_joint' 5 | publish_rate: 50.0 # defaults to 50 6 | 7 | pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] 8 | twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] 9 | cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests 10 | 11 | #wheel_separation_h : 0.4 12 | #wheel_radius : 0.11 13 | wheel_separation_h_multiplier: 0.257 # calibration parameter for odometory, needed for test. 14 | -------------------------------------------------------------------------------- /steer_drive_controller/test/common/config/steerbot_hw_sim.yaml: -------------------------------------------------------------------------------- 1 | steerbot_hw_sim: 2 | rear_wheel : 'rear_wheel_joint' 3 | front_steer : 'front_steer_joint' 4 | 5 | rear_wheels: ['rear_right_wheel_joint', 'rear_left_wheel_joint'] 6 | front_wheels: ['front_right_wheel_joint', 'front_left_wheel_joint'] 7 | front_steers: ['front_right_steer_joint', 'front_left_steer_joint'] 8 | 9 | wheel_separation_h : 0.20 10 | -------------------------------------------------------------------------------- /steer_drive_controller/test/common/include/steerbot.h: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (C) 2013, PAL Robotics S.L. 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 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of PAL Robotics, Inc. nor the names of its 12 | // contributors may be used to endorse or promote products derived from 13 | // this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | // POSSIBILITY OF SUCH DAMAGE. 26 | ////////////////////////////////////////////////////////////////////////////// 27 | 28 | // NOTE: The contents of this file have been taken largely from the ros_control wiki tutorials 29 | 30 | // ROS 31 | #include 32 | #include 33 | #include 34 | 35 | // ros_control 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | // NaN 43 | #include 44 | 45 | // ostringstream 46 | #include 47 | 48 | enum INDEX_WHEEL { 49 | INDEX_RIGHT = 0, 50 | INDEX_LEFT = 1 51 | }; 52 | 53 | class Steerbot : public hardware_interface::RobotHW 54 | { 55 | public: 56 | Steerbot() 57 | : running_(true) 58 | , start_srv_(nh_.advertiseService("start", &Steerbot::startCallback, this)) 59 | , stop_srv_(nh_.advertiseService("stop", &Steerbot::stopCallback, this)) 60 | , ns_("steerbot_hw_sim/") 61 | { 62 | // Intialize raw data 63 | this->cleanUp(); 64 | this->getJointNames(nh_); 65 | this->registerHardwareInterfaces(); 66 | 67 | nh_.getParam(ns_ + "wheel_separation_w", wheel_separation_w_); 68 | nh_.getParam(ns_ + "wheel_separation_h", wheel_separation_h_); 69 | ROS_INFO_STREAM("wheel_separation_w in test steerbot= " << wheel_separation_w_); 70 | ROS_INFO_STREAM("wheel_separation_h in test steerbot= " << wheel_separation_h_); 71 | } 72 | 73 | ros::Time getTime() const {return ros::Time::now();} 74 | ros::Duration getPeriod() const {return ros::Duration(0.01);} 75 | 76 | void read() 77 | { 78 | std::ostringstream os; 79 | // directly get from controller 80 | os << rear_wheel_jnt_vel_cmd_ << ", "; 81 | os << front_steer_jnt_pos_cmd_ << ", "; 82 | 83 | // convert to each joint velocity 84 | //-- differential drive 85 | for (unsigned int i = 0; i < virtual_rear_wheel_jnt_vel_cmd_.size(); i++) 86 | { 87 | virtual_rear_wheel_jnt_vel_cmd_[i] = rear_wheel_jnt_vel_cmd_; 88 | os << virtual_rear_wheel_jnt_vel_cmd_[i] << ", "; 89 | } 90 | 91 | //-- ackerman link 92 | const double h = wheel_separation_h_; 93 | const double w = wheel_separation_w_; 94 | virtual_front_steer_jnt_pos_cmd_[INDEX_RIGHT] = atan2(2*h*tan(front_steer_jnt_pos_cmd_), 2*h + w/2.0*tan(front_steer_jnt_pos_cmd_)); 95 | virtual_front_steer_jnt_pos_cmd_[INDEX_LEFT] = atan2(2*h*tan(front_steer_jnt_pos_cmd_), 2*h - w/2.0*tan(front_steer_jnt_pos_cmd_)); 96 | 97 | for (unsigned int i = 0; i < virtual_front_steer_jnt_pos_cmd_.size(); i++) 98 | { 99 | os << virtual_front_steer_jnt_pos_cmd_[i] << ", "; 100 | } 101 | 102 | if (rear_wheel_jnt_vel_cmd_ != 0.0 || front_steer_jnt_pos_cmd_ != 0.0) 103 | ROS_INFO_STREAM("Commands for joints: " << os.str()); 104 | 105 | } 106 | 107 | void write() 108 | { 109 | std::ostringstream os; 110 | if (running_) 111 | { 112 | // wheels 113 | rear_wheel_jnt_pos_ += rear_wheel_jnt_vel_*getPeriod().toSec(); 114 | rear_wheel_jnt_vel_ = rear_wheel_jnt_vel_cmd_; 115 | for (unsigned int i = 0; i < virtual_rear_wheel_jnt_vel_cmd_.size(); i++) 116 | { 117 | // Note that pos_[i] will be NaN for one more cycle after we start(), 118 | // but that is consistent with the knowledge we have about the state 119 | // of the robot. 120 | virtual_rear_wheel_jnt_pos_[i] += virtual_rear_wheel_jnt_vel_[i]*getPeriod().toSec(); 121 | virtual_rear_wheel_jnt_vel_[i] = virtual_rear_wheel_jnt_vel_cmd_[i]; 122 | } 123 | 124 | // steers 125 | front_steer_jnt_pos_ = front_steer_jnt_pos_cmd_; 126 | for (unsigned int i = 0; i < virtual_front_steer_jnt_pos_cmd_.size(); i++) 127 | { 128 | virtual_front_steer_jnt_pos_[i] = virtual_front_steer_jnt_pos_cmd_[i]; 129 | } 130 | 131 | // directly get from controller 132 | os << rear_wheel_jnt_vel_cmd_ << ", "; 133 | os << front_steer_jnt_pos_cmd_ << ", "; 134 | 135 | // convert to each joint velocity 136 | //-- differential drive 137 | for (unsigned int i = 0; i < virtual_rear_wheel_jnt_pos_.size(); i++) 138 | { 139 | os << virtual_rear_wheel_jnt_pos_[i] << ", "; 140 | } 141 | 142 | //-- ackerman link 143 | for (unsigned int i = 0; i < virtual_front_steer_jnt_pos_.size(); i++) 144 | { 145 | os << virtual_front_steer_jnt_pos_[i] << ", "; 146 | } 147 | } 148 | else 149 | { 150 | // wheels 151 | rear_wheel_jnt_pos_= std::numeric_limits::quiet_NaN(); 152 | rear_wheel_jnt_vel_= std::numeric_limits::quiet_NaN(); 153 | std::fill_n(virtual_rear_wheel_jnt_pos_.begin(), virtual_rear_wheel_jnt_pos_.size(), std::numeric_limits::quiet_NaN()); 154 | std::fill_n(virtual_rear_wheel_jnt_vel_.begin(), virtual_rear_wheel_jnt_vel_.size(), std::numeric_limits::quiet_NaN()); 155 | // steers 156 | front_steer_jnt_pos_= std::numeric_limits::quiet_NaN(); 157 | front_steer_jnt_vel_= std::numeric_limits::quiet_NaN(); 158 | std::fill_n(virtual_front_steer_jnt_pos_.begin(), virtual_front_steer_jnt_pos_.size(), std::numeric_limits::quiet_NaN()); 159 | std::fill_n(virtual_front_steer_jnt_vel_.begin(), virtual_front_steer_jnt_vel_.size(), std::numeric_limits::quiet_NaN()); 160 | 161 | // wheels 162 | os << rear_wheel_jnt_pos_ << ", "; 163 | os << rear_wheel_jnt_vel_ << ", "; 164 | for (unsigned int i = 0; i < virtual_rear_wheel_jnt_pos_.size(); i++) 165 | { 166 | os << virtual_rear_wheel_jnt_pos_[i] << ", "; 167 | } 168 | 169 | // steers 170 | os << front_steer_jnt_pos_ << ", "; 171 | os << front_steer_jnt_vel_ << ", "; 172 | for (unsigned int i = 0; i < virtual_front_steer_jnt_pos_.size(); i++) 173 | { 174 | os << virtual_front_steer_jnt_pos_[i] << ", "; 175 | } 176 | } 177 | ROS_INFO_STREAM("running_ = " << running_ << ". commands are " << os.str()); 178 | } 179 | 180 | bool startCallback(std_srvs::Empty::Request& /*req*/, std_srvs::Empty::Response& /*res*/) 181 | { 182 | ROS_INFO_STREAM("running_ = " << running_ << "."); 183 | running_ = true; 184 | return true; 185 | } 186 | 187 | bool stopCallback(std_srvs::Empty::Request& /*req*/, std_srvs::Empty::Response& /*res*/) 188 | { 189 | ROS_INFO_STREAM("running_ = " << running_ << "."); 190 | running_ = false; 191 | return true; 192 | } 193 | 194 | private: 195 | 196 | void cleanUp() 197 | { 198 | // wheel 199 | //-- wheel joint names 200 | rear_wheel_jnt_name_.empty(); 201 | virtual_rear_wheel_jnt_names_.clear(); 202 | //-- actual rear wheel joint 203 | rear_wheel_jnt_pos_ = 0; 204 | rear_wheel_jnt_vel_ = 0; 205 | rear_wheel_jnt_eff_ = 0; 206 | rear_wheel_jnt_vel_cmd_ = 0; 207 | //-- virtual rear wheel joint 208 | virtual_rear_wheel_jnt_pos_.clear(); 209 | virtual_rear_wheel_jnt_vel_.clear(); 210 | virtual_rear_wheel_jnt_eff_.clear(); 211 | virtual_rear_wheel_jnt_vel_cmd_.clear(); 212 | //-- virtual front wheel joint 213 | virtual_front_wheel_jnt_pos_.clear(); 214 | virtual_front_wheel_jnt_vel_.clear(); 215 | virtual_front_wheel_jnt_eff_.clear(); 216 | virtual_front_wheel_jnt_vel_cmd_.clear(); 217 | 218 | // steer 219 | //-- steer joint names 220 | front_steer_jnt_name_.empty(); 221 | virtual_front_steer_jnt_names_.clear(); 222 | //-- front steer joint 223 | front_steer_jnt_pos_ = 0; 224 | front_steer_jnt_vel_ = 0; 225 | front_steer_jnt_eff_ = 0; 226 | front_steer_jnt_pos_cmd_ = 0; 227 | //-- virtual wheel joint 228 | virtual_front_steer_jnt_pos_.clear(); 229 | virtual_front_steer_jnt_vel_.clear(); 230 | virtual_front_steer_jnt_eff_.clear(); 231 | virtual_front_steer_jnt_pos_cmd_.clear(); 232 | } 233 | 234 | void getJointNames(ros::NodeHandle &_nh) 235 | { 236 | this->getWheelJointNames(_nh); 237 | this->getSteerJointNames(_nh); 238 | } 239 | 240 | void getWheelJointNames(ros::NodeHandle &_nh) 241 | { 242 | // wheel joint to get linear command 243 | _nh.getParam(ns_ + "rear_wheel", rear_wheel_jnt_name_); 244 | 245 | // virtual wheel joint for gazebo con_rol 246 | _nh.getParam(ns_ + "rear_wheels", virtual_rear_wheel_jnt_names_); 247 | int dof = virtual_rear_wheel_jnt_names_.size(); 248 | virtual_rear_wheel_jnt_pos_.resize(dof); 249 | virtual_rear_wheel_jnt_vel_.resize(dof); 250 | virtual_rear_wheel_jnt_eff_.resize(dof); 251 | virtual_rear_wheel_jnt_vel_cmd_.resize(dof); 252 | 253 | _nh.getParam(ns_ + "front_wheels", virtual_front_wheel_jnt_names_); 254 | dof = virtual_front_wheel_jnt_names_.size(); 255 | virtual_front_wheel_jnt_pos_.resize(dof); 256 | virtual_front_wheel_jnt_vel_.resize(dof); 257 | virtual_front_wheel_jnt_eff_.resize(dof); 258 | virtual_front_wheel_jnt_vel_cmd_.resize(dof); 259 | } 260 | 261 | void getSteerJointNames(ros::NodeHandle &_nh) 262 | { 263 | // steer joint to get angular command 264 | _nh.getParam(ns_ + "front_steer", front_steer_jnt_name_); 265 | 266 | // virtual steer joint for gazebo control 267 | _nh.getParam(ns_ + "front_steers", virtual_front_steer_jnt_names_); 268 | 269 | const int dof = virtual_front_steer_jnt_names_.size(); 270 | virtual_front_steer_jnt_pos_.resize(dof); 271 | virtual_front_steer_jnt_vel_.resize(dof); 272 | virtual_front_steer_jnt_eff_.resize(dof); 273 | virtual_front_steer_jnt_pos_cmd_.resize(dof); 274 | } 275 | 276 | void registerHardwareInterfaces() 277 | { 278 | this->registerSteerInterface(); 279 | this->registerWheelInterface(); 280 | 281 | // register mapped interface to ros_control 282 | registerInterface(&jnt_state_interface_); 283 | registerInterface(&rear_wheel_jnt_vel_cmd_interface_); 284 | registerInterface(&front_steer_jnt_pos_cmd_interface_); 285 | } 286 | 287 | void registerWheelInterface() 288 | { 289 | // actual wheel joints 290 | this->registerInterfaceHandles( 291 | jnt_state_interface_, rear_wheel_jnt_vel_cmd_interface_, 292 | rear_wheel_jnt_name_, rear_wheel_jnt_pos_, rear_wheel_jnt_vel_, rear_wheel_jnt_eff_, rear_wheel_jnt_vel_cmd_); 293 | 294 | // virtual rear wheel joints 295 | for (int i = 0; i < virtual_rear_wheel_jnt_names_.size(); i++) 296 | { 297 | this->registerInterfaceHandles( 298 | jnt_state_interface_, rear_wheel_jnt_vel_cmd_interface_, 299 | virtual_rear_wheel_jnt_names_[i], virtual_rear_wheel_jnt_pos_[i], virtual_rear_wheel_jnt_vel_[i], virtual_rear_wheel_jnt_eff_[i], virtual_rear_wheel_jnt_vel_cmd_[i]); 300 | } 301 | // virtual front wheel joints 302 | for (int i = 0; i < virtual_front_wheel_jnt_names_.size(); i++) 303 | { 304 | this->registerInterfaceHandles( 305 | jnt_state_interface_, rear_wheel_jnt_vel_cmd_interface_, 306 | virtual_front_wheel_jnt_names_[i], virtual_front_wheel_jnt_pos_[i], virtual_front_wheel_jnt_vel_[i], virtual_front_wheel_jnt_eff_[i], virtual_front_wheel_jnt_vel_cmd_[i]); 307 | } 308 | } 309 | 310 | void registerSteerInterface() 311 | { 312 | // actual steer joints 313 | this->registerInterfaceHandles( 314 | jnt_state_interface_, front_steer_jnt_pos_cmd_interface_, 315 | front_steer_jnt_name_, front_steer_jnt_pos_, front_steer_jnt_vel_, front_steer_jnt_eff_, front_steer_jnt_pos_cmd_); 316 | 317 | // virtual steer joints 318 | for (int i = 0; i < virtual_front_steer_jnt_names_.size(); i++) 319 | { 320 | this->registerInterfaceHandles( 321 | jnt_state_interface_, front_steer_jnt_pos_cmd_interface_, 322 | virtual_front_steer_jnt_names_[i], virtual_front_steer_jnt_pos_[i], virtual_front_steer_jnt_vel_[i], virtual_front_steer_jnt_eff_[i], virtual_front_steer_jnt_pos_cmd_[i]); 323 | } 324 | } 325 | 326 | void registerInterfaceHandles( 327 | hardware_interface::JointStateInterface& _jnt_state_interface, 328 | hardware_interface::JointCommandInterface& _jnt_cmd_interface, 329 | const std::string _jnt_name, double& _jnt_pos, double& _jnt_vel, double& _jnt_eff, double& _jnt_cmd) 330 | { 331 | // register joint (both JointState and CommandJoint) 332 | this->registerJointStateInterfaceHandle(_jnt_state_interface, _jnt_name, 333 | _jnt_pos, _jnt_vel, _jnt_eff); 334 | this->registerCommandJointInterfaceHandle(_jnt_state_interface, _jnt_cmd_interface, 335 | _jnt_name, _jnt_cmd); 336 | } 337 | 338 | void registerJointStateInterfaceHandle( 339 | hardware_interface::JointStateInterface& _jnt_state_interface, 340 | const std::string _jnt_name, double& _jnt_pos, double& _jnt_vel, double& _jnt_eff) 341 | { 342 | hardware_interface::JointStateHandle state_handle(_jnt_name, 343 | &_jnt_pos, 344 | &_jnt_vel, 345 | &_jnt_eff); 346 | _jnt_state_interface.registerHandle(state_handle); 347 | 348 | ROS_INFO_STREAM("Registered joint '" << _jnt_name << " ' in the JointStateInterface"); 349 | } 350 | 351 | void registerCommandJointInterfaceHandle( 352 | hardware_interface::JointStateInterface& _jnt_state_interface, 353 | hardware_interface::JointCommandInterface& _jnt_cmd_interface, 354 | const std::string _jnt_name, double& _jnt_cmd) 355 | { 356 | // joint command 357 | hardware_interface::JointHandle _handle(_jnt_state_interface.getHandle(_jnt_name), 358 | &_jnt_cmd); 359 | _jnt_cmd_interface.registerHandle(_handle); 360 | 361 | ROS_INFO_STREAM("Registered joint '" << _jnt_name << " ' in the CommandJointInterface"); 362 | } 363 | 364 | private: 365 | // common 366 | hardware_interface::JointStateInterface jnt_state_interface_;// rear wheel 367 | //-- actual joint(single actuator) 368 | //---- joint name 369 | std::string rear_wheel_jnt_name_; 370 | //---- joint interface parameters 371 | double rear_wheel_jnt_pos_; 372 | double rear_wheel_jnt_vel_; 373 | double rear_wheel_jnt_eff_; 374 | //---- joint interface command 375 | double rear_wheel_jnt_vel_cmd_; 376 | //---- Hardware interface: joint 377 | hardware_interface::VelocityJointInterface rear_wheel_jnt_vel_cmd_interface_; 378 | //hardware_interface::JointStateInterface wheel_jnt_state_interface_; 379 | // 380 | //-- virtual joints(two rear wheels) 381 | //---- joint name 382 | std::vector virtual_rear_wheel_jnt_names_; 383 | //---- joint interface parameters 384 | std::vector virtual_rear_wheel_jnt_pos_; 385 | std::vector virtual_rear_wheel_jnt_vel_; 386 | std::vector virtual_rear_wheel_jnt_eff_; 387 | //---- joint interface command 388 | std::vector virtual_rear_wheel_jnt_vel_cmd_; 389 | //-- virtual joints(two front wheels) 390 | //---- joint name 391 | std::vector virtual_front_wheel_jnt_names_; 392 | //---- joint interface parameters 393 | std::vector virtual_front_wheel_jnt_pos_; 394 | std::vector virtual_front_wheel_jnt_vel_; 395 | std::vector virtual_front_wheel_jnt_eff_; 396 | //---- joint interface command 397 | std::vector virtual_front_wheel_jnt_vel_cmd_; 398 | 399 | // front steer 400 | //-- actual joint(single actuator) 401 | //---- joint name 402 | std::string front_steer_jnt_name_; 403 | //---- joint interface parameters 404 | double front_steer_jnt_pos_; 405 | double front_steer_jnt_vel_; 406 | double front_steer_jnt_eff_; 407 | //---- joint interface command 408 | double front_steer_jnt_pos_cmd_; 409 | //---- Hardware interface: joint 410 | hardware_interface::PositionJointInterface front_steer_jnt_pos_cmd_interface_; 411 | //hardware_interface::JointStateInterface steer_jnt_state_interface_; 412 | // 413 | //-- virtual joints(two steers) 414 | //---- joint name 415 | std::vector virtual_front_steer_jnt_names_; 416 | //---- joint interface parameters 417 | std::vector virtual_front_steer_jnt_pos_; 418 | std::vector virtual_front_steer_jnt_vel_; 419 | std::vector virtual_front_steer_jnt_eff_; 420 | //---- joint interface command 421 | std::vector virtual_front_steer_jnt_pos_cmd_; 422 | 423 | // Wheel separation, wrt the midpoint of the wheel width: 424 | double wheel_separation_w_; 425 | // Wheel separation, wrt the midpoint of the wheel width: 426 | double wheel_separation_h_; 427 | 428 | std::string ns_; 429 | bool running_; 430 | 431 | ros::NodeHandle nh_; 432 | ros::ServiceServer start_srv_; 433 | ros::ServiceServer stop_srv_; 434 | }; 435 | -------------------------------------------------------------------------------- /steer_drive_controller/test/common/include/test_common.h: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (C) 2013, PAL Robotics S.L. 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 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of PAL Robotics, Inc. nor the names of its 12 | // contributors may be used to endorse or promote products derived from 13 | // this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | // POSSIBILITY OF SUCH DAMAGE. 26 | ////////////////////////////////////////////////////////////////////////////// 27 | 28 | /// \author Bence Magyar 29 | 30 | #include 31 | 32 | #include 33 | 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | 40 | #include 41 | 42 | // Floating-point value comparison threshold 43 | const double EPS = 0.01; 44 | const double POSITION_TOLERANCE = 0.01; // 1 cm-s precision 45 | const double VELOCITY_TOLERANCE = 0.02; // 2 cm-s-1 precision 46 | const double ANGULAR_VELOCITY_TOLERANCE = 0.05; // 3 deg-s-1 precision 47 | const double JERK_LINEAR_VELOCITY_TOLERANCE = 0.10; // 10 cm-s-1 precision 48 | const double JERK_ANGULAR_VELOCITY_TOLERANCE = 0.05; // 3 deg-s-1 precision 49 | const double ORIENTATION_TOLERANCE = 0.03; // 0.57 degree precision 50 | 51 | class SteerDriveControllerTest : public ::testing::Test 52 | { 53 | public: 54 | 55 | SteerDriveControllerTest() 56 | : cmd_pub(nh.advertise("cmd_vel", 100)) 57 | , odom_sub(nh.subscribe("odom", 100, &SteerDriveControllerTest::odomCallback, this)) 58 | , start_srv(nh.serviceClient("start")) 59 | , stop_srv(nh.serviceClient("stop")) 60 | { 61 | } 62 | 63 | ~SteerDriveControllerTest() 64 | { 65 | odom_sub.shutdown(); 66 | } 67 | 68 | nav_msgs::Odometry getLastOdom(){ return last_odom; } 69 | void publish(geometry_msgs::Twist cmd_vel){ cmd_pub.publish(cmd_vel); } 70 | bool isControllerAlive(){ return (odom_sub.getNumPublishers() > 0) && (cmd_pub.getNumSubscribers() > 0); } 71 | 72 | void start(){ std_srvs::Empty srv; start_srv.call(srv); } 73 | void stop(){ std_srvs::Empty srv; stop_srv.call(srv); } 74 | 75 | private: 76 | ros::NodeHandle nh; 77 | ros::Publisher cmd_pub; 78 | ros::Subscriber odom_sub; 79 | nav_msgs::Odometry last_odom; 80 | 81 | ros::ServiceClient start_srv; 82 | ros::ServiceClient stop_srv; 83 | 84 | void odomCallback(const nav_msgs::Odometry& odom) 85 | { 86 | ROS_INFO_STREAM("Callback reveived: pos.x: " << odom.pose.pose.position.x 87 | << ", orient.z: " << odom.pose.pose.orientation.z 88 | << ", lin_est: " << odom.twist.twist.linear.x 89 | << ", ang_est: " << odom.twist.twist.angular.z); 90 | last_odom = odom; 91 | } 92 | }; 93 | 94 | inline tf::Quaternion tfQuatFromGeomQuat(const geometry_msgs::Quaternion& quat) 95 | { 96 | return tf::Quaternion(quat.x, quat.y, quat.z, quat.w); 97 | } 98 | -------------------------------------------------------------------------------- /steer_drive_controller/test/common/launch/steer_drive_common.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 16 | 17 | 18 | 21 | 22 | 23 | 37 | 38 | -------------------------------------------------------------------------------- /steer_drive_controller/test/common/launch/view_steerbot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 19 | 20 | 21 | 24 | 25 | 26 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /steer_drive_controller/test/common/rviz/display.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1 10 | Splitter Ratio: 0.5 11 | Tree Height: 775 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.588679 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: "" 31 | Visualization Manager: 32 | Class: "" 33 | Displays: 34 | - Alpha: 0.5 35 | Cell Size: 1 36 | Class: rviz/Grid 37 | Color: 160; 160; 164 38 | Enabled: true 39 | Line Style: 40 | Line Width: 0.03 41 | Value: Lines 42 | Name: Grid 43 | Normal Cell Count: 0 44 | Offset: 45 | X: 0 46 | Y: 0 47 | Z: 0 48 | Plane: XY 49 | Plane Cell Count: 10 50 | Reference Frame: 51 | Value: true 52 | - Alpha: 1 53 | Class: rviz/RobotModel 54 | Collision Enabled: false 55 | Enabled: true 56 | Links: 57 | All Links Enabled: true 58 | Expand Joint Details: false 59 | Expand Link Details: false 60 | Expand Tree: false 61 | Link Tree Style: Links in Alphabetic Order 62 | base_link: 63 | Alpha: 1 64 | Show Axes: false 65 | Show Trail: false 66 | Value: true 67 | camera_link: 68 | Alpha: 1 69 | Show Axes: false 70 | Show Trail: false 71 | Value: true 72 | front_bottom_lrf: 73 | Alpha: 1 74 | Show Axes: false 75 | Show Trail: false 76 | Value: true 77 | front_top_3durg: 78 | Alpha: 1 79 | Show Axes: false 80 | Show Trail: false 81 | Value: true 82 | front_top_3durg_rgbd_optical_frame: 83 | Alpha: 1 84 | Show Axes: false 85 | Show Trail: false 86 | left_front_wheel: 87 | Alpha: 1 88 | Show Axes: false 89 | Show Trail: false 90 | Value: true 91 | left_rear_wheel: 92 | Alpha: 1 93 | Show Axes: false 94 | Show Trail: false 95 | Value: true 96 | merged_lrf: 97 | Alpha: 1 98 | Show Axes: false 99 | Show Trail: false 100 | rear_bottom_lrf: 101 | Alpha: 1 102 | Show Axes: false 103 | Show Trail: false 104 | Value: true 105 | right_front_wheel: 106 | Alpha: 1 107 | Show Axes: false 108 | Show Trail: false 109 | Value: true 110 | right_rear_wheel: 111 | Alpha: 1 112 | Show Axes: false 113 | Show Trail: false 114 | Value: true 115 | steer: 116 | Alpha: 1 117 | Show Axes: false 118 | Show Trail: false 119 | steer_left: 120 | Alpha: 1 121 | Show Axes: false 122 | Show Trail: false 123 | steer_right: 124 | Alpha: 1 125 | Show Axes: false 126 | Show Trail: false 127 | Name: RobotModel 128 | Robot Description: robot_description 129 | TF Prefix: "" 130 | Update Interval: 0 131 | Value: true 132 | Visual Enabled: true 133 | - Class: rviz/TF 134 | Enabled: true 135 | Frame Timeout: 15 136 | Frames: 137 | All Enabled: true 138 | base_link: 139 | Value: true 140 | camera_link: 141 | Value: true 142 | front_bottom_lrf: 143 | Value: true 144 | front_top_3durg: 145 | Value: true 146 | front_top_3durg_rgbd_optical_frame: 147 | Value: true 148 | left_front_wheel: 149 | Value: true 150 | left_rear_wheel: 151 | Value: true 152 | merged_lrf: 153 | Value: true 154 | rear_bottom_lrf: 155 | Value: true 156 | right_front_wheel: 157 | Value: true 158 | right_rear_wheel: 159 | Value: true 160 | steer: 161 | Value: true 162 | steer_left: 163 | Value: true 164 | steer_right: 165 | Value: true 166 | Marker Scale: 1 167 | Name: TF 168 | Show Arrows: true 169 | Show Axes: true 170 | Show Names: true 171 | Tree: 172 | base_link: 173 | camera_link: 174 | {} 175 | front_bottom_lrf: 176 | {} 177 | front_top_3durg: 178 | front_top_3durg_rgbd_optical_frame: 179 | {} 180 | left_rear_wheel: 181 | {} 182 | merged_lrf: 183 | {} 184 | rear_bottom_lrf: 185 | {} 186 | right_rear_wheel: 187 | {} 188 | steer: 189 | {} 190 | steer_left: 191 | left_front_wheel: 192 | {} 193 | steer_right: 194 | right_front_wheel: 195 | {} 196 | Update Interval: 0 197 | Value: true 198 | Enabled: true 199 | Global Options: 200 | Background Color: 48; 48; 48 201 | Fixed Frame: base_link 202 | Frame Rate: 30 203 | Name: root 204 | Tools: 205 | - Class: rviz/Interact 206 | Hide Inactive Objects: true 207 | - Class: rviz/MoveCamera 208 | - Class: rviz/Select 209 | - Class: rviz/FocusCamera 210 | - Class: rviz/Measure 211 | - Class: rviz/SetInitialPose 212 | Topic: /initialpose 213 | - Class: rviz/SetGoal 214 | Topic: /move_base_simple/goal 215 | - Class: rviz/PublishPoint 216 | Single click: true 217 | Topic: /clicked_point 218 | Value: true 219 | Views: 220 | Current: 221 | Class: rviz/Orbit 222 | Distance: 2.57916 223 | Enable Stereo Rendering: 224 | Stereo Eye Separation: 0.06 225 | Stereo Focal Distance: 1 226 | Swap Stereo Eyes: false 227 | Value: false 228 | Focal Point: 229 | X: -0.129039 230 | Y: -0.142336 231 | Z: 0.339095 232 | Name: Current View 233 | Near Clip Distance: 0.01 234 | Pitch: 0.420398 235 | Target Frame: base_link 236 | Value: Orbit (rviz) 237 | Yaw: 0.805398 238 | Saved: ~ 239 | Window Geometry: 240 | Displays: 241 | collapsed: false 242 | Height: 1056 243 | Hide Left Dock: false 244 | Hide Right Dock: true 245 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007510000003efc0100000002fb0000000800540069006d0065010000000000000751000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000005e10000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 246 | Selection: 247 | collapsed: false 248 | Time: 249 | collapsed: false 250 | Tool Properties: 251 | collapsed: false 252 | Views: 253 | collapsed: true 254 | Width: 1873 255 | X: 47 256 | Y: 24 257 | -------------------------------------------------------------------------------- /steer_drive_controller/test/common/src/steerbot.cpp: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (C) 2013, PAL Robotics S.L. 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 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of PAL Robotics, Inc. nor the names of its 12 | // contributors may be used to endorse or promote products derived from 13 | // this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | // POSSIBILITY OF SUCH DAMAGE. 26 | ////////////////////////////////////////////////////////////////////////////// 27 | 28 | // NOTE: The contents of this file have been taken largely from the ros_control wiki tutorials 29 | 30 | // ROS 31 | #include 32 | 33 | // ros_control 34 | #include 35 | 36 | #include "./../include/steerbot.h" 37 | 38 | int main(int argc, char **argv) 39 | { 40 | ros::init(argc, argv, "steerbot"); 41 | ros::NodeHandle nh; 42 | 43 | Steerbot robot; 44 | ROS_WARN_STREAM("period: " << robot.getPeriod().toSec()); 45 | controller_manager::ControllerManager cm(&robot, nh); 46 | 47 | ros::Rate rate(1.0 / robot.getPeriod().toSec()); 48 | ros::AsyncSpinner spinner(1); 49 | spinner.start(); 50 | while(ros::ok()) 51 | { 52 | ROS_WARN_STREAM("period: " << robot.getPeriod().toSec()); 53 | robot.read(); 54 | cm.update(robot.getTime(), robot.getPeriod()); 55 | robot.write(); 56 | rate.sleep(); 57 | } 58 | spinner.stop(); 59 | 60 | return 0; 61 | } 62 | -------------------------------------------------------------------------------- /steer_drive_controller/test/common/urdf/steerbot.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 72 | 73 | 74 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | Gazebo/Green 89 | 90 | 91 | 92 | Gazebo/Purple 93 | 94 | 95 | 96 | -------------------------------------------------------------------------------- /steer_drive_controller/test/common/urdf/steerbot_sphere_wheels.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 72 | 73 | 74 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | Gazebo/Green 89 | 90 | 91 | 92 | Gazebo/Purple 93 | 94 | 95 | 96 | -------------------------------------------------------------------------------- /steer_drive_controller/test/common/urdf/wheel.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 37 | 38 | 39 | 40 | Gazebo/Yellow 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 1 108 | 1 109 | 110 | 111 | 112 | 113 | 1 114 | 1 115 | 116 | 117 | 118 | Gazebo/Blue 119 | 120 | 121 | Gazebo/Red 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 1 158 | 1 159 | 160 | 161 | 162 | Gazebo/Yellow 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 1 199 | 1 200 | 201 | 202 | 203 | Gazebo/Red 204 | 205 | 206 | 207 | -------------------------------------------------------------------------------- /steer_drive_controller/test/common/urdf/wheel_sphere.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 37 | 38 | 39 | 40 | Gazebo/Yellow 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 1 108 | 1 109 | 110 | 111 | 112 | 113 | 1 114 | 1 115 | 116 | 117 | 118 | Gazebo/Blue 119 | 120 | 121 | Gazebo/Red 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 1 158 | 1 159 | 160 | 161 | 162 | Gazebo/Yellow 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 1 199 | 1 200 | 201 | 202 | 203 | Gazebo/Red 204 | 205 | 206 | 207 | -------------------------------------------------------------------------------- /steer_drive_controller/test/steer_drive_controller_default_odom_frame_test/steer_drive_controller_default_odom_frame.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 9 | 10 | 11 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /steer_drive_controller/test/steer_drive_controller_default_odom_frame_test/steer_drive_controller_default_odom_frame_test.cpp: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (C) 2015, Locus Robotics Corp. 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 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of PAL Robotics, Inc. nor the names of its 12 | // contributors may be used to endorse or promote products derived from 13 | // this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | // POSSIBILITY OF SUCH DAMAGE. 26 | ////////////////////////////////////////////////////////////////////////////// 27 | 28 | /// \author Eric Tappan 29 | 30 | #include "../common/include/test_common.h" 31 | #include 32 | 33 | // TEST CASES 34 | TEST_F(SteerDriveControllerTest, testOdomFrame) 35 | { 36 | // wait for ROS 37 | while(!isControllerAlive()) 38 | { 39 | ros::Duration(0.1).sleep(); 40 | } 41 | // set up tf listener 42 | tf::TransformListener listener; 43 | ros::Duration(2.0).sleep(); 44 | // check the original odom frame doesn't exist 45 | EXPECT_TRUE(listener.frameExists("odom")); 46 | } 47 | 48 | TEST_F(SteerDriveControllerTest, testOdomTopic) 49 | { 50 | // wait for ROS 51 | while(!isControllerAlive()) 52 | { 53 | ros::Duration(0.1).sleep(); 54 | } 55 | 56 | // get an odom message 57 | nav_msgs::Odometry odom_msg = getLastOdom(); 58 | // check its frame_id 59 | ASSERT_STREQ(odom_msg.header.frame_id.c_str(), "odom"); 60 | } 61 | 62 | int main(int argc, char** argv) 63 | { 64 | testing::InitGoogleTest(&argc, argv); 65 | ros::init(argc, argv, "steer_drive_controller_default_odom_frame_test"); 66 | 67 | ros::AsyncSpinner spinner(1); 68 | spinner.start(); 69 | int ret = RUN_ALL_TESTS(); 70 | spinner.stop(); 71 | ros::shutdown(); 72 | return ret; 73 | } 74 | -------------------------------------------------------------------------------- /steer_drive_controller/test/steer_drive_controller_fail_test/steer_drive_controller_fail_test.cpp: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (C) 2013, PAL Robotics S.L. 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 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of PAL Robotics, Inc. nor the names of its 12 | // contributors may be used to endorse or promote products derived from 13 | // this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | // POSSIBILITY OF SUCH DAMAGE. 26 | ////////////////////////////////////////////////////////////////////////////// 27 | 28 | /// \author Paul Mathieu 29 | 30 | #include "../common/include/test_common.h" 31 | 32 | // TEST CASES 33 | TEST_F(SteerDriveControllerTest, testWrongJointName) 34 | { 35 | // the controller should never be alive 36 | int secs = 0; 37 | while(!isControllerAlive() && secs < 5) 38 | { 39 | ros::Duration(1.0).sleep(); 40 | secs++; 41 | } 42 | // give up and assume controller load failure after 5 seconds 43 | EXPECT_GE(secs, 5); 44 | } 45 | 46 | int main(int argc, char** argv) 47 | { 48 | testing::InitGoogleTest(&argc, argv); 49 | ros::init(argc, argv, "steer_drive_controller_fail_test"); 50 | 51 | ros::AsyncSpinner spinner(1); 52 | spinner.start(); 53 | int ret = RUN_ALL_TESTS(); 54 | spinner.stop(); 55 | ros::shutdown(); 56 | return ret; 57 | } 58 | -------------------------------------------------------------------------------- /steer_drive_controller/test/steer_drive_controller_fail_test/steer_drive_controller_wrong.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /steer_drive_controller/test/steer_drive_controller_fail_test/steerbot_wrong.yaml: -------------------------------------------------------------------------------- 1 | steerbot_controller: 2 | type: "steer_drive_controller/SteerDriveController" 3 | rear_wheel : 'this_joint_does_not_exist' 4 | front_steer : 'front_steer_joint' 5 | publish_rate: 50.0 # defaults to 50 6 | pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] 7 | twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] 8 | cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests 9 | -------------------------------------------------------------------------------- /steer_drive_controller/test/steer_drive_controller_limits_test/steer_drive_controller_limits.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /steer_drive_controller/test/steer_drive_controller_limits_test/steer_drive_controller_limits_test.cpp: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (C) 2013, PAL Robotics S.L. 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 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of PAL Robotics, Inc. nor the names of its 12 | // contributors may be used to endorse or promote products derived from 13 | // this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | // POSSIBILITY OF SUCH DAMAGE. 26 | ////////////////////////////////////////////////////////////////////////////// 27 | 28 | /// \author Paul Mathieu 29 | 30 | #include "../common/include/test_common.h" 31 | 32 | // TEST CASES 33 | TEST_F(SteerDriveControllerTest, testLinearJerkLimits) 34 | { 35 | // wait for ROS 36 | while(!isControllerAlive()) 37 | { 38 | ros::Duration(0.1).sleep(); 39 | } 40 | // zero everything before test 41 | geometry_msgs::Twist cmd_vel; 42 | cmd_vel.linear.x = 0.0; 43 | cmd_vel.angular.z = 0.0; 44 | publish(cmd_vel); 45 | ros::Duration(2.0).sleep(); 46 | // get initial odom 47 | nav_msgs::Odometry old_odom = getLastOdom(); 48 | // send a big command 49 | cmd_vel.linear.x = 10.0; 50 | publish(cmd_vel); 51 | // wait for a while 52 | ros::Duration(0.5).sleep(); 53 | 54 | nav_msgs::Odometry new_odom = getLastOdom(); 55 | 56 | // check if the robot speed is now 0.37m.s-1 57 | EXPECT_NEAR(new_odom.twist.twist.linear.x, 0.37, JERK_LINEAR_VELOCITY_TOLERANCE); 58 | EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), EPS); 59 | 60 | cmd_vel.linear.x = 0.0; 61 | publish(cmd_vel); 62 | } 63 | 64 | TEST_F(SteerDriveControllerTest, testLinearAccelerationLimits) 65 | { 66 | // wait for ROS 67 | while(!isControllerAlive()) 68 | { 69 | ros::Duration(0.1).sleep(); 70 | } 71 | // zero everything before test 72 | geometry_msgs::Twist cmd_vel; 73 | cmd_vel.linear.x = 0.0; 74 | cmd_vel.angular.z = 0.0; 75 | publish(cmd_vel); 76 | ros::Duration(2.0).sleep(); 77 | // get initial odom 78 | nav_msgs::Odometry old_odom = getLastOdom(); 79 | // send a big command 80 | cmd_vel.linear.x = 10.0; 81 | publish(cmd_vel); 82 | // wait for a while 83 | ros::Duration(0.5).sleep(); 84 | 85 | nav_msgs::Odometry new_odom = getLastOdom(); 86 | 87 | // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s 88 | EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), 0.5 + VELOCITY_TOLERANCE); 89 | EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), EPS); 90 | 91 | cmd_vel.linear.x = 0.0; 92 | publish(cmd_vel); 93 | } 94 | 95 | TEST_F(SteerDriveControllerTest, testLinearVelocityLimits) 96 | { 97 | // wait for ROS 98 | while(!isControllerAlive()) 99 | { 100 | ros::Duration(0.1).sleep(); 101 | } 102 | // zero everything before test 103 | geometry_msgs::Twist cmd_vel; 104 | cmd_vel.linear.x = 0.0; 105 | cmd_vel.angular.z = 0.0; 106 | publish(cmd_vel); 107 | ros::Duration(2.0).sleep(); 108 | // get initial odom 109 | nav_msgs::Odometry old_odom = getLastOdom(); 110 | // send a big command 111 | cmd_vel.linear.x = 10.0; 112 | publish(cmd_vel); 113 | // wait for a while 114 | ros::Duration(5.0).sleep(); 115 | 116 | nav_msgs::Odometry new_odom = getLastOdom(); 117 | 118 | // check if the robot speed is now 1.0 m.s-1, the limit 119 | EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), 1.0 + VELOCITY_TOLERANCE); 120 | EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), EPS); 121 | 122 | cmd_vel.linear.x = 0.0; 123 | publish(cmd_vel); 124 | } 125 | 126 | TEST_F(SteerDriveControllerTest, testAngularJerkLimits) 127 | { 128 | // wait for ROS 129 | while(!isControllerAlive()) 130 | { 131 | ros::Duration(0.1).sleep(); 132 | } 133 | // zero everything before test 134 | geometry_msgs::Twist cmd_vel; 135 | cmd_vel.linear.x = 0.0; 136 | cmd_vel.angular.z = 0.0; 137 | publish(cmd_vel); 138 | ros::Duration(2.0).sleep(); 139 | // get initial odom 140 | nav_msgs::Odometry old_odom = getLastOdom(); 141 | // send a big command 142 | cmd_vel.angular.z = 10.0; 143 | // send linear command too 144 | // because sending only angular command doesn't actuate wheels for steer drive mechanism 145 | cmd_vel.linear.x = 0.1; 146 | publish(cmd_vel); 147 | // wait for a while 148 | ros::Duration(0.5).sleep(); 149 | 150 | nav_msgs::Odometry new_odom = getLastOdom(); 151 | 152 | // check if the robot speed is now 0.18rad.s-1 153 | EXPECT_NEAR(new_odom.twist.twist.angular.z, 0.18, JERK_ANGULAR_VELOCITY_TOLERANCE); 154 | // check if the robot speed is now 0.1m.s-1 155 | EXPECT_NEAR(new_odom.twist.twist.linear.x, 0.1, VELOCITY_TOLERANCE); 156 | 157 | cmd_vel.angular.z = 0.0; 158 | publish(cmd_vel); 159 | } 160 | 161 | TEST_F(SteerDriveControllerTest, testAngularAccelerationLimits) 162 | { 163 | // wait for ROS 164 | while(!isControllerAlive()) 165 | { 166 | ros::Duration(0.1).sleep(); 167 | } 168 | // zero everything before test 169 | geometry_msgs::Twist cmd_vel; 170 | cmd_vel.linear.x = 0.0; 171 | cmd_vel.angular.z = 0.0; 172 | publish(cmd_vel); 173 | ros::Duration(2.0).sleep(); 174 | // get initial odom 175 | nav_msgs::Odometry old_odom = getLastOdom(); 176 | // send a big command 177 | cmd_vel.angular.z = 10.0; 178 | // send linear command too 179 | // because sending only angular command doesn't actuate wheels for steer drive mechanism 180 | cmd_vel.linear.x = 0.1; 181 | publish(cmd_vel); 182 | // wait for a while 183 | ros::Duration(0.5).sleep(); 184 | 185 | nav_msgs::Odometry new_odom = getLastOdom(); 186 | 187 | // check if the robot speed is now 0.25rad.s-1, which is 0.5.s-2 * 0.5s 188 | EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), 0.25 + VELOCITY_TOLERANCE); 189 | // check if the robot speed is now 0.1m.s-1 190 | EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), 0.1 + VELOCITY_TOLERANCE); 191 | 192 | cmd_vel.angular.z = 0.0; 193 | publish(cmd_vel); 194 | } 195 | 196 | TEST_F(SteerDriveControllerTest, testAngularVelocityLimits) 197 | { 198 | // wait for ROS 199 | while(!isControllerAlive()) 200 | { 201 | ros::Duration(0.1).sleep(); 202 | } 203 | // zero everything before test 204 | geometry_msgs::Twist cmd_vel; 205 | cmd_vel.linear.x = 0.0; 206 | cmd_vel.angular.z = 0.0; 207 | publish(cmd_vel); 208 | ros::Duration(2.0).sleep(); 209 | // get initial odom 210 | nav_msgs::Odometry old_odom = getLastOdom(); 211 | cmd_vel.angular.z = 10.0; 212 | // send linear command too 213 | // because sending only angular command doesn't actuate wheels for steer drive mechanism 214 | cmd_vel.linear.x = 0.1; 215 | publish(cmd_vel); 216 | // wait for a while 217 | ros::Duration(5.0).sleep(); 218 | 219 | nav_msgs::Odometry new_odom = getLastOdom(); 220 | 221 | // check if the robot speed is now 0.5rad.s-1, the limit 222 | EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), 0.5 + ANGULAR_VELOCITY_TOLERANCE); 223 | // check if the robot speed is now 0.1m.s-1 224 | EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), 0.1 + VELOCITY_TOLERANCE); 225 | 226 | cmd_vel.angular.z = 0.0; 227 | publish(cmd_vel); 228 | } 229 | 230 | int main(int argc, char** argv) 231 | { 232 | testing::InitGoogleTest(&argc, argv); 233 | ros::init(argc, argv, "steer_drive_controller_limits_test"); 234 | 235 | ros::AsyncSpinner spinner(1); 236 | spinner.start(); 237 | //ros::Duration(0.5).sleep(); 238 | int ret = RUN_ALL_TESTS(); 239 | spinner.stop(); 240 | ros::shutdown(); 241 | return ret; 242 | } 243 | -------------------------------------------------------------------------------- /steer_drive_controller/test/steer_drive_controller_limits_test/steerbot_limits.yaml: -------------------------------------------------------------------------------- 1 | steerbot_controller: 2 | linear: 3 | x: 4 | has_velocity_limits: true 5 | min_velocity: -0.5 6 | max_velocity: 1.0 7 | has_acceleration_limits: true 8 | min_acceleration: -0.5 9 | max_acceleration: 1.0 10 | has_jerk_limits: true 11 | max_jerk: 5.0 12 | angular: 13 | z: 14 | has_velocity_limits: true 15 | max_velocity: 0.5 16 | has_acceleration_limits: true 17 | max_acceleration: 0.5 18 | has_jerk_limits: true 19 | max_jerk: 2.5 20 | -------------------------------------------------------------------------------- /steer_drive_controller/test/steer_drive_controller_nan_test/steer_drive_controller_nan.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /steer_drive_controller/test/steer_drive_controller_nan_test/steer_drive_controller_nan_test.cpp: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (C) 2014, PAL Robotics S.L. 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 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of PAL Robotics, Inc. nor the names of its 12 | // contributors may be used to endorse or promote products derived from 13 | // this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | // POSSIBILITY OF SUCH DAMAGE. 26 | ////////////////////////////////////////////////////////////////////////////// 27 | 28 | /// \author Enrique Fernandez 29 | 30 | #include "../common/include/test_common.h" 31 | 32 | // NaN 33 | #include 34 | 35 | // TEST CASES 36 | TEST_F(SteerDriveControllerTest, testNaN) 37 | { 38 | // wait for ROS 39 | while(!isControllerAlive()) 40 | { 41 | ros::Duration(0.1).sleep(); 42 | } 43 | // zero everything before test 44 | geometry_msgs::Twist cmd_vel; 45 | cmd_vel.linear.x = 0.0; 46 | cmd_vel.angular.z = 0.0; 47 | publish(cmd_vel); 48 | ros::Duration(2.0).sleep(); 49 | 50 | // send a command 51 | cmd_vel.linear.x = 0.1; 52 | ros::Duration(2.0).sleep(); 53 | 54 | // stop robot (will generate NaN) 55 | stop(); 56 | ros::Duration(2.0).sleep(); 57 | 58 | nav_msgs::Odometry odom = getLastOdom(); 59 | 60 | EXPECT_NE(std::isnan(odom.twist.twist.linear.x), true); 61 | EXPECT_NE(std::isnan(odom.twist.twist.angular.z), true); 62 | EXPECT_NE(std::isnan(odom.pose.pose.position.x), true); 63 | EXPECT_NE(std::isnan(odom.pose.pose.position.y), true); 64 | EXPECT_NE(std::isnan(odom.pose.pose.orientation.z), true); 65 | EXPECT_NE(std::isnan(odom.pose.pose.orientation.w), true); 66 | 67 | // start robot 68 | start(); 69 | ros::Duration(2.0).sleep(); 70 | 71 | odom = getLastOdom(); 72 | 73 | EXPECT_NE(std::isnan(odom.twist.twist.linear.x), true); 74 | EXPECT_NE(std::isnan(odom.twist.twist.angular.z), true); 75 | EXPECT_NE(std::isnan(odom.pose.pose.position.x), true); 76 | EXPECT_NE(std::isnan(odom.pose.pose.position.y), true); 77 | EXPECT_NE(std::isnan(odom.pose.pose.orientation.z), true); 78 | EXPECT_NE(std::isnan(odom.pose.pose.orientation.w), true); 79 | } 80 | 81 | int main(int argc, char** argv) 82 | { 83 | testing::InitGoogleTest(&argc, argv); 84 | ros::init(argc, argv, "steer_drive_controller_nan_test"); 85 | 86 | ros::AsyncSpinner spinner(1); 87 | spinner.start(); 88 | int ret = RUN_ALL_TESTS(); 89 | spinner.stop(); 90 | ros::shutdown(); 91 | return ret; 92 | } 93 | -------------------------------------------------------------------------------- /steer_drive_controller/test/steer_drive_controller_no_steer_test/steer_drive_controller_no_steer.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /steer_drive_controller/test/steer_drive_controller_no_steer_test/steerbot_no_steer.yaml: -------------------------------------------------------------------------------- 1 | steerbot_controller: 2 | type: "steer_drive_controller/SteerDriveController" 3 | rear_wheel : 'rear_wheel_joint' 4 | front_steer : '' 5 | publish_rate: 50.0 # defaults to 50 6 | pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] 7 | twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] 8 | cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests 9 | -------------------------------------------------------------------------------- /steer_drive_controller/test/steer_drive_controller_no_wheel_test/steer_drive_controller_no_wheel.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /steer_drive_controller/test/steer_drive_controller_no_wheel_test/steerbot_no_wheel.yaml: -------------------------------------------------------------------------------- 1 | steerbot_controller: 2 | type: "steer_drive_controller/SteerDriveController" 3 | rear_wheel : '' 4 | front_steer : 'front_steer_joint' 5 | publish_rate: 50.0 # defaults to 50 6 | pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] 7 | twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] 8 | cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests 9 | -------------------------------------------------------------------------------- /steer_drive_controller/test/steer_drive_controller_odom_frame_test/steer_drive_controller_odom_frame.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | steerbot_controller: 8 | odom_frame_id: new_odom 9 | 10 | 11 | 12 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /steer_drive_controller/test/steer_drive_controller_odom_frame_test/steer_drive_controller_odom_frame_test.cpp: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (C) 2015, Locus Robotics Corp. 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 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of PAL Robotics, Inc. nor the names of its 12 | // contributors may be used to endorse or promote products derived from 13 | // this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | // POSSIBILITY OF SUCH DAMAGE. 26 | ////////////////////////////////////////////////////////////////////////////// 27 | 28 | /// \author Eric Tappan 29 | 30 | #include "../common/include/test_common.h" 31 | #include 32 | 33 | // TEST CASES 34 | TEST_F(SteerDriveControllerTest, testNoOdomFrame) 35 | { 36 | // wait for ROS 37 | while(!isControllerAlive()) 38 | { 39 | ros::Duration(0.1).sleep(); 40 | } 41 | // set up tf listener 42 | tf::TransformListener listener; 43 | ros::Duration(2.0).sleep(); 44 | // check the original odom frame doesn't exist 45 | EXPECT_FALSE(listener.frameExists("odom")); 46 | } 47 | 48 | TEST_F(SteerDriveControllerTest, testNewOdomFrame) 49 | { 50 | // wait for ROS 51 | while(!isControllerAlive()) 52 | { 53 | ros::Duration(0.1).sleep(); 54 | } 55 | // set up tf listener 56 | tf::TransformListener listener; 57 | ros::Duration(2.0).sleep(); 58 | // check the new_odom frame does exist 59 | EXPECT_TRUE(listener.frameExists("new_odom")); 60 | } 61 | 62 | TEST_F(SteerDriveControllerTest, testOdomTopic) 63 | { 64 | // wait for ROS 65 | while(!isControllerAlive()) 66 | { 67 | ros::Duration(0.1).sleep(); 68 | } 69 | 70 | ros::Duration(2.0).sleep(); 71 | // get an odom message 72 | nav_msgs::Odometry odom_msg = getLastOdom(); 73 | // check its frame_id 74 | ASSERT_STREQ(odom_msg.header.frame_id.c_str(), "new_odom"); 75 | } 76 | 77 | int main(int argc, char** argv) 78 | { 79 | testing::InitGoogleTest(&argc, argv); 80 | ros::init(argc, argv, "steer_drive_controller_odom_frame_test"); 81 | 82 | ros::AsyncSpinner spinner(1); 83 | spinner.start(); 84 | int ret = RUN_ALL_TESTS(); 85 | spinner.stop(); 86 | ros::shutdown(); 87 | return ret; 88 | } 89 | -------------------------------------------------------------------------------- /steer_drive_controller/test/steer_drive_controller_odom_tf_test/steer_drive_controller_odom_tf.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | steerbot_controller: 7 | enable_odom_tf: False 8 | 9 | 10 | 11 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /steer_drive_controller/test/steer_drive_controller_odom_tf_test/steer_drive_controller_odom_tf_test.cpp: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (C) 2014, PAL Robotics S.L. 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 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of PAL Robotics, Inc. nor the names of its 12 | // contributors may be used to endorse or promote products derived from 13 | // this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | // POSSIBILITY OF SUCH DAMAGE. 26 | ////////////////////////////////////////////////////////////////////////////// 27 | 28 | /// \author Bence Magyar 29 | 30 | #include "../common/include/test_common.h" 31 | #include 32 | 33 | // TEST CASES 34 | TEST_F(SteerDriveControllerTest, testNoOdomFrame) 35 | { 36 | // wait for ROS 37 | while(!isControllerAlive()) 38 | { 39 | ros::Duration(0.1).sleep(); 40 | } 41 | // set up tf listener 42 | tf::TransformListener listener; 43 | ros::Duration(2.0).sleep(); 44 | // check the odom frame doesn't exist 45 | EXPECT_FALSE(listener.frameExists("odom")); 46 | } 47 | 48 | int main(int argc, char** argv) 49 | { 50 | testing::InitGoogleTest(&argc, argv); 51 | ros::init(argc, argv, "steer_drive_controller_odom_tf_test"); 52 | 53 | ros::AsyncSpinner spinner(1); 54 | spinner.start(); 55 | int ret = RUN_ALL_TESTS(); 56 | spinner.stop(); 57 | ros::shutdown(); 58 | return ret; 59 | } 60 | -------------------------------------------------------------------------------- /steer_drive_controller/test/steer_drive_controller_open_loop_test/steer_drive_controller_open_loop.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /steer_drive_controller/test/steer_drive_controller_open_loop_test/steerbot_open_loop.yaml: -------------------------------------------------------------------------------- 1 | steerbot_controller: 2 | open_loop: true 3 | -------------------------------------------------------------------------------- /steer_drive_controller/test/steer_drive_controller_radius_param_fail_test/steer_drive_controller_radius_param_fail.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | 12 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /steer_drive_controller/test/steer_drive_controller_radius_param_test/steer_drive_controller_radius_param.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | 12 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /steer_drive_controller/test/steer_drive_controller_separation_param_test/steer_drive_controller_separation_param.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /steer_drive_controller/test/steer_drive_controller_test/steer_drive_controller.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /steer_drive_controller/test/steer_drive_controller_test/steer_drive_controller_test.cpp: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (C) 2013, PAL Robotics S.L. 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 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of PAL Robotics, Inc. nor the names of its 12 | // contributors may be used to endorse or promote products derived from 13 | // this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | // POSSIBILITY OF SUCH DAMAGE. 26 | ////////////////////////////////////////////////////////////////////////////// 27 | 28 | /// \author Bence Magyar 29 | 30 | #include "../common/include/test_common.h" 31 | #include 32 | 33 | // TEST CASES 34 | TEST_F(SteerDriveControllerTest, testForward) 35 | { 36 | // wait for ROS 37 | while(!isControllerAlive()) 38 | { 39 | ros::Duration(0.1).sleep(); 40 | } 41 | // zero everything before test 42 | geometry_msgs::Twist cmd_vel; 43 | cmd_vel.linear.x = 0.0; 44 | cmd_vel.angular.z = 0.0; 45 | publish(cmd_vel); 46 | ros::Duration(0.1).sleep(); 47 | // get initial odom 48 | nav_msgs::Odometry old_odom = getLastOdom(); 49 | // send a velocity command of 0.1 m/s 50 | cmd_vel.linear.x = 0.1; 51 | publish(cmd_vel); 52 | // wait for 10s 53 | ros::Duration(10.0).sleep(); 54 | 55 | nav_msgs::Odometry new_odom = getLastOdom(); 56 | 57 | // check if the robot traveled 1 meter in XY plane, changes in z should be ~~0 58 | const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x; 59 | const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y; 60 | const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z; 61 | EXPECT_NEAR(sqrt(dx*dx + dy*dy), 1.0, POSITION_TOLERANCE); 62 | EXPECT_LT(fabs(dz), EPS); 63 | 64 | // convert to rpy and test that way 65 | double roll_old, pitch_old, yaw_old; 66 | double roll_new, pitch_new, yaw_new; 67 | tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old); 68 | tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new); 69 | EXPECT_LT(fabs(roll_new - roll_old), EPS); 70 | EXPECT_LT(fabs(pitch_new - pitch_old), EPS); 71 | EXPECT_LT(fabs(yaw_new - yaw_old), EPS); 72 | EXPECT_NEAR(fabs(new_odom.twist.twist.linear.x), cmd_vel.linear.x, EPS); 73 | EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS); 74 | EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS); 75 | 76 | EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS); 77 | EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS); 78 | EXPECT_LT(fabs(new_odom.twist.twist.angular.z), EPS); 79 | } 80 | 81 | TEST_F(SteerDriveControllerTest, testTurn) 82 | { 83 | // wait for ROS 84 | while(!isControllerAlive()) 85 | { 86 | ros::Duration(0.1).sleep(); 87 | } 88 | // zero everything before test 89 | geometry_msgs::Twist cmd_vel; 90 | cmd_vel.linear.x = 0.0; 91 | cmd_vel.angular.z = 0.0; 92 | publish(cmd_vel); 93 | ros::Duration(0.1).sleep(); 94 | // get initial odom 95 | nav_msgs::Odometry old_odom = getLastOdom(); 96 | // send a velocity command 97 | cmd_vel.angular.z = M_PI/10.0; 98 | // send linear command too 99 | // because sending only angular command doesn't actuate wheels for steer drive mechanism 100 | cmd_vel.linear.x = 0.1; 101 | publish(cmd_vel); 102 | // wait for 10s 103 | ros::Duration(10.0).sleep(); 104 | 105 | nav_msgs::Odometry new_odom = getLastOdom(); 106 | 107 | // check if the robot rotated PI around z, changes in x should be ~~0 and in y should be y_answer 108 | double x_answer = 0.0; 109 | double y_answer = 2.0 * cmd_vel.linear.x / cmd_vel.angular.z; // R = v/w, D = 2R 110 | EXPECT_NEAR(fabs(new_odom.pose.pose.position.x - old_odom.pose.pose.position.x), x_answer, POSITION_TOLERANCE); 111 | EXPECT_NEAR(fabs(new_odom.pose.pose.position.y - old_odom.pose.pose.position.y), y_answer, POSITION_TOLERANCE); 112 | EXPECT_LT(fabs(new_odom.pose.pose.position.z - old_odom.pose.pose.position.z), EPS); 113 | 114 | // convert to rpy and test that way 115 | double roll_old, pitch_old, yaw_old; 116 | double roll_new, pitch_new, yaw_new; 117 | tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old); 118 | tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new); 119 | EXPECT_LT(fabs(roll_new - roll_old), EPS); 120 | EXPECT_LT(fabs(pitch_new - pitch_old), EPS); 121 | EXPECT_NEAR(fabs(yaw_new - yaw_old), M_PI, ORIENTATION_TOLERANCE); 122 | 123 | EXPECT_NEAR(new_odom.twist.twist.linear.x, 0.1, VELOCITY_TOLERANCE); 124 | EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS); 125 | EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS); 126 | 127 | EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS); 128 | EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS); 129 | EXPECT_NEAR(fabs(new_odom.twist.twist.angular.z), M_PI/10.0, EPS); 130 | } 131 | 132 | TEST_F(SteerDriveControllerTest, testOdomFrame) 133 | { 134 | // wait for ROS 135 | while(!isControllerAlive()) 136 | { 137 | ros::Duration(0.1).sleep(); 138 | } 139 | // set up tf listener 140 | tf::TransformListener listener; 141 | ros::Duration(2.0).sleep(); 142 | // check the odom frame exist 143 | EXPECT_TRUE(listener.frameExists("odom")); 144 | } 145 | 146 | int main(int argc, char** argv) 147 | { 148 | testing::InitGoogleTest(&argc, argv); 149 | ros::init(argc, argv, "steer_drive_controller_test"); 150 | 151 | ros::AsyncSpinner spinner(1); 152 | spinner.start(); 153 | //ros::Duration(0.5).sleep(); 154 | int ret = RUN_ALL_TESTS(); 155 | spinner.stop(); 156 | ros::shutdown(); 157 | return ret; 158 | } 159 | -------------------------------------------------------------------------------- /steer_drive_controller/test/steer_drive_controller_timeout_test/steer_drive_controller_timeout.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /steer_drive_controller/test/steer_drive_controller_timeout_test/steer_drive_controller_timeout_test.cpp: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (C) 2013, PAL Robotics S.L. 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 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of PAL Robotics, Inc. nor the names of its 12 | // contributors may be used to endorse or promote products derived from 13 | // this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | // POSSIBILITY OF SUCH DAMAGE. 26 | ////////////////////////////////////////////////////////////////////////////// 27 | 28 | /// \author Paul Mathieu 29 | 30 | #include "../common/include/test_common.h" 31 | 32 | // TEST CASES 33 | TEST_F(SteerDriveControllerTest, testTimeout) 34 | { 35 | // wait for ROS 36 | while(!isControllerAlive()) 37 | { 38 | ros::Duration(0.1).sleep(); 39 | } 40 | // zero everything before test 41 | geometry_msgs::Twist cmd_vel; 42 | cmd_vel.linear.x = 0.0; 43 | cmd_vel.angular.z = 0.0; 44 | publish(cmd_vel); 45 | // give some time to the controller to react to the command 46 | ros::Duration(0.1).sleep(); 47 | // get initial odom 48 | nav_msgs::Odometry old_odom = getLastOdom(); 49 | // send a velocity command of 1 m/s 50 | cmd_vel.linear.x = 1.0; 51 | publish(cmd_vel); 52 | // wait a bit 53 | ros::Duration(3.0).sleep(); 54 | 55 | nav_msgs::Odometry new_odom = getLastOdom(); 56 | 57 | // check if the robot has stopped after 0.5s, thus covering less than 0.5s*1.0m.s-1 + some (big) tolerance 58 | EXPECT_LT(fabs(new_odom.pose.pose.position.x - old_odom.pose.pose.position.x), 0.8); 59 | } 60 | 61 | int main(int argc, char** argv) 62 | { 63 | testing::InitGoogleTest(&argc, argv); 64 | ros::init(argc, argv, "steer_drive_controller_timeout_test"); 65 | 66 | ros::AsyncSpinner spinner(1); 67 | spinner.start(); 68 | int ret = RUN_ALL_TESTS(); 69 | spinner.stop(); 70 | ros::shutdown(); 71 | return ret; 72 | } 73 | -------------------------------------------------------------------------------- /steer_drive_controller/test/steer_drive_controller_timeout_test/steerbot_timeout.yaml: -------------------------------------------------------------------------------- 1 | steerbot_controller: 2 | cmd_vel_timeout: 0.5 3 | -------------------------------------------------------------------------------- /steer_drive_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(steer_drive_ros) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /steer_drive_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | steer_drive_ros 4 | 0.1.0 5 | Steer driving meta package for ROS. 6 | 7 | Masaru Morita 8 | BSD 9 | https://github.com/CIR-KIT/steer_drive_ros.git 10 | https://github.com/CIR-KIT/steer_drive_ros/issues 11 | CIR-KIT 12 | 13 | catkin 14 | steer_bot_hardware_gazebo 15 | steer_drive_controller 16 | stepback_and_steerturn_recovery 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /stepback_and_steerturn_recovery/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(stepback_and_steerturn_recovery) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | cmake_modules 7 | roscpp 8 | tf 9 | costmap_2d 10 | nav_core 11 | pluginlib 12 | base_local_planner 13 | ) 14 | 15 | find_package(Eigen REQUIRED) 16 | include_directories( 17 | include 18 | ${catkin_INCLUDE_DIRS} 19 | ${EIGEN_INCLUDE_DIRS} 20 | ) 21 | add_definitions(${EIGEN_DEFINITIONS}) 22 | 23 | catkin_package( 24 | CATKIN_DEPENDS 25 | roscpp 26 | pluginlib 27 | ) 28 | 29 | add_library(${PROJECT_NAME} src/stepback_and_steerturn_recovery.cpp) 30 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 31 | 32 | install(TARGETS ${PROJECT_NAME} 33 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 34 | ) 35 | 36 | install(FILES stepback_and_steerturn_recovery_plugin.xml 37 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 38 | ) 39 | -------------------------------------------------------------------------------- /stepback_and_steerturn_recovery/README.md: -------------------------------------------------------------------------------- 1 | #stepback_and_steerturn_recovery 2 | ## 仕様 3 | - ざっくりしたものはissue参照→[Recovery の挙動について #10](https://github.com/CIR-KIT/third_robot_pkg/issues/10) 4 | - 実際には，これよりもうちょっと色々やっている． 5 | - 左に障害物があったら，「右旋回」→「直進」→「左旋回」を行うことで進路クリアを図っている 6 | - 移動量、回転量で制御．制御量はパラメータで設定可能． 7 | - 移動中リアルタイムにcostmapを参照し，衝突前に停止する．許容距離はパラメータで設定可能． 8 | - 「後退→旋回→直進→反対旋回」or「後退→旋回」をパラメータで選択可能. 9 | - 行動前に障害物をチェック．障害物が存在したら下記に従って目標移動距離を修正． 10 | - 修正前: hogehoge_length (パラメータ) 11 | - 修正後: 障害物までの距離 - obstacle_patience (パラメータ) 12 | - 各動作毎にタイムアウトを設定可能． 13 | 14 | ## Topic 15 | ### Publish 16 | - `/recover_run` 17 | - Type: std_msgs::Bool 18 | - リカバリ開始時にtrue, リカバリ終了時にfalseを`publish`． 19 | 20 | ## 挙動 21 | - 動画サンプル→https://www.youtube.com/watch?v=j6GID9XuiiU 22 | 23 | ## 使い方 24 | ### 事前設定 25 | - まずは`catkin_make`する． 26 | 27 | - navigation の `planner.yaml`(NavfanROSやTrajectoryPlannerROSの設定が入っているファイル)に，以下のパラメータを追加する． 28 | 29 | ``` yaml: 30 | recovery_behaviour_enabled: true 31 | 32 | recovery_behaviors: 33 | - {name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery} 34 | - {name: stepback_and_steerturn_recovery, type: stepback_and_steerturn_recovery/StepBackAndSteerTurnRecovery} 35 | - {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery} 36 | 37 | stepback_and_steerturn_recovery: 38 | # 最初の一回だけ旋回したい場合にtrue 39 | only_single_steering: true 40 | # リカバリ行動の試行回数[回] 41 | trial_times : 3 42 | # 障害物までの許容距離[m]． 43 | #-- 移動中に，移動方向に対して最も近い障害物がこの距離以内に出現したら停止する． 44 | obstacle_patience : 0.3 45 | #-- 移動中に，障害物を確認する頻度[回/sec] 46 | obstacle_check_frequency: 5.0 47 | # 障害物探索時の角度の分解能[rad] costmapアクセス数を低減したいときに調整する． 48 | sim_angle_resolution: 0.1 49 | # 障害物探索時のsim更新周期[回/sec] costmapアクセス回数を低減したいときに調整する． 50 | simulation_frequency : 5 51 | # back(初回後退時の速度[m/s], 移動距離[m], タイムアウト[sec]) 52 | linear_vel_back : -0.3 53 | step_back_length : 1.0 54 | step_back_timeout : 15.0 55 | # steer(旋回時の速度[rad/s]と目標回転角度[rad], タイムアウト[sec]) 56 | linear_vel_steer : 0.3 57 | angular_speed_steer : 0.5 58 | turn_angle : 1.5 59 | steering_timeout : 15.0 60 | # forward(旋回→直進→旋回の直進時の速度[m/s]と目標移動距離[m], タイムアウト[sec]) 61 | linear_vel_forward : 0.3 62 | step_forward_length : 1.0 63 | step_forward_timeout: 15.0 64 | 65 | ``` 66 | 67 | ### 使用時挙動 68 | 69 | - `move_base`立ち上げ時に以下のメッセージが出ていれば初期化成功 70 | 71 | ```bash 72 | [ INFO] [1476905975.890241798, 6973.810000000]: Initialized with only_single_steering = false 73 | [ INFO] [1476905975.890302193, 6973.810000000]: Initialized with recovery_trial_times = 3 74 | [ INFO] [1476905975.890322274, 6973.810000000]: Initialized with obstacle_patience = 0.50 75 | [ INFO] [1476905975.890340282, 6973.810000000]: Initialized with obstacle_check_frequency = 6.00 76 | [ INFO] [1476905975.890340282, 6973.810000000]: Initialized with simulation_frequency = 5.10 77 | [ INFO] [1476905975.890357877, 6973.810000000]: Initialized with sim_angle_resolution = 0.15 78 | [ INFO] [1476905975.890378780, 6973.810000000]: Initialized with linear_vel_back = -0.30, step_back_length = 8.00, step_back_steering = 3.00 79 | [ INFO] [1476905975.890576838, 6973.810000000]: Initialized with linear_vel_steer = 0.50, angular_speed_steer = 0.50, turn_angle = 2.00, steering_timeout = 3.10 80 | [ INFO] [1476905975.890605697, 6973.810000000]: Initialized with linear_vel_forward = 0.30, step_forward_length = 8.00, step_forward_timeout = 3.20 81 | 82 | ``` 83 | 84 | - パラメータサーバに次のように登録されていればOK 85 | 86 | ```bash 87 | rosparam list | grep stepback_and_steerturn_recovery 88 | /move_base/stepback_and_steerturn_recovery/angular_speed_steer 89 | /move_base/stepback_and_steerturn_recovery/linear_vel_back 90 | /move_base/stepback_and_steerturn_recovery/linear_vel_forward 91 | /move_base/stepback_and_steerturn_recovery/linear_vel_steer 92 | /move_base/stepback_and_steerturn_recovery/obstacle_check_frequency 93 | /move_base/stepback_and_steerturn_recovery/obstacle_patience 94 | /move_base/stepback_and_steerturn_recovery/only_single_steering 95 | /move_base/stepback_and_steerturn_recovery/sim_angle_resolution 96 | /move_base/stepback_and_steerturn_recovery/simulation_frequency 97 | /move_base/stepback_and_steerturn_recovery/steering_timeout 98 | /move_base/stepback_and_steerturn_recovery/step_back_length 99 | /move_base/stepback_and_steerturn_recovery/step_back_timeout 100 | /move_base/stepback_and_steerturn_recovery/step_forward_length 101 | /move_base/stepback_and_steerturn_recovery/step_forward_timeout 102 | /move_base/stepback_and_steerturn_recovery/trial_times 103 | /move_base/stepback_and_steerturn_recovery/turn_angle 104 | ``` 105 | 106 | - リカバリ行動に入ったら以下のようなメッセージが出る． 107 | - 1秒に1回，移動横行と障害物までの最短距離を出力する． 108 | - 障害物発見時はWARNINGで表示． 109 | 110 | ```bash 111 | [ INFO] [1476868375.829523527, 2480.590000000]: ***************************************************** 112 | [ INFO] [1476868375.829690887, 2480.590000000]: **********Start StepBackAndSteerRecovery!!!********** 113 | [ INFO] [1476868375.829753797, 2480.590000000]: ***************************************************** 114 | [ INFO] [1476868375.829794323, 2480.590000000]: ==== 1 th recovery trial ==== 115 | [ INFO] [1476868383.643012049, 2488.400000000]: min dist to obstacle = 1.59 [m] in BACKWARD 116 | [ INFO] [1476868384.840226745, 2489.590000000]: min dist to obstacle = 0.92 [m] in BACKWARD 117 | [ WARN] [1476868386.036249305, 2490.790000000]: obstacle detected at BACKWARD 118 | [ WARN] [1476868386.036298028, 2490.790000000]: min dist to obstacle = 0.48 [m] in BACKWARD 119 | [ INFO] [1476868386.036321913, 2490.790000000]: complete step back 120 | [ INFO] [1476868387.007138030, 2491.760000000]: min_l = 12.22 [m], min_r = 5.98 [m] 121 | [ INFO] [1476868387.007252925, 2491.760000000]: attempting to turn left at the 1st turn 122 | [ INFO] [1476868387.610307113, 2492.360000000]: min dist to obstacle = 11.09 [m] in FORWARD_LEFT 123 | [ INFO] [1476868388.005182918, 2492.760000000]: complete the 1st turn 124 | [ INFO] [1476868388.005297723, 2492.760000000]: attemping step forward 125 | [ INFO] [1476868388.611079357, 2493.360000000]: min dist to obstacle = 8.73 [m] in FORWARD 126 | [ INFO] [1476868389.212381216, 2493.960000000]: min dist to obstacle = 7.70 [m] in FORWARD 127 | [ INFO] [1476868390.413571989, 2495.160000000]: min dist to obstacle = 5.98 [m] in FORWARD 128 | [ INFO] [1476868391.010836290, 2495.760000000]: complete step forward 129 | [ INFO] [1476868391.010889155, 2495.760000000]: attempting second turn 130 | [ INFO] [1476868391.013244061, 2495.760000000]: min dist to obstacle = 25.86 [m] in FORWARD_RIGHT 131 | [ INFO] [1476868392.008923908, 2496.760000000]: complete second turn 132 | [ INFO] [1476868392.993798006, 2497.740000000]: break recovery because the robot got clearance 133 | [ INFO] [1476868392.993863847, 2497.740000000]: ***************************************************** 134 | [ INFO] [1476868392.993893442, 2497.740000000]: **********Finish StepBackAndSteerRecovery!!********** 135 | [ INFO] [1476868392.993917387, 2497.740000000]: ***************************************************** 136 | ``` 137 | 138 | ### WARNINGの種類 139 | 140 | - 移動前に障害物検知 141 | ```bash 142 | [ WARN] [1476906022.832819115, 7020.720000000]: obstacle detected before moving FORWARD 143 | [ WARN] [1476906022.832909665, 7020.720000000]: min dist to obstacle = 2.97 [m] in FORWARD 144 | [ WARN] [1476906022.832949896, 7020.720000000]: moving length is switched from 8.00 [m] to 1.97 in FORWARD 145 | ``` 146 | 147 | - 移動中に障害物検知 148 | ```bash 149 | [ WARN] [1476868386.036249305, 2490.790000000]: obstacle detected at BACKWARD 150 | [ WARN] [1476868386.036298028, 2490.790000000]: min dist to obstacle = 0.48 [m] in BACKWARD 151 | ``` 152 | 153 | - タイムアウト 154 | ```bash 155 | [ WARN] [1476906019.469352572, 7017.360000000]: time out at BACKWARD 156 | [ WARN] [1476906019.469436583, 7017.360000000]: 3.00 [sec] elapsed. 157 | ``` 158 | -------------------------------------------------------------------------------- /stepback_and_steerturn_recovery/config/backwards.yaml: -------------------------------------------------------------------------------- 1 | # Configuration for version of this recovery where we back up slowly about half a meter 2 | linear_x: -0.3 3 | linear_y: 0.0 4 | angular_z: 0.0 5 | duration: 1.5 6 | -------------------------------------------------------------------------------- /stepback_and_steerturn_recovery/config/planner_sample.yaml: -------------------------------------------------------------------------------- 1 | recovery_behaviour_enabled: true 2 | 3 | recovery_behaviors: 4 | - {name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery} 5 | - {name: stepback_and_steerturn_recovery, type: stepback_and_steerturn_recovery/StepBackAndSteerTurnRecovery} 6 | - {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery} 7 | 8 | step_back_and_max_steer_recovery: 9 | # 最初の一回だけ旋回したい場合にtrue 10 | only_single_steering: true 11 | # リカバリ行動の試行回数[回] 12 | trial_times : 3 13 | # 障害物までの許容距離[m]． 14 | #-- 移動中に，移動方向に対して最も近い障害物がこの距離以内に出現したら停止する． 15 | obstacle_patience : 0.3 16 | #-- 移動中に，障害物を確認する頻度[回/sec] 17 | obstacle_check_frequency: 5.0 18 | # 障害物探索時の角度の分解能[rad] costmapアクセス回数を低減したいときに調整する． 19 | sim_angle_resolution: 0.1 20 | # 障害物探索時のsim更新周期 [回/sec] costmapアクセス回数を低減したいときに調整する． 21 | simulation_frequency : 5 22 | # back(初回後退時の速度[m/s], 移動距離[m], タイムアウト[sec]) 23 | linear_vel_back : -0.3 24 | step_back_length : 1.0 25 | step_back_timeout : 15.0 26 | # steer(旋回時の速度[rad/s], 目標回転角度[rad], タイムアウト[sec]) 27 | linear_vel_steer : 0.3 28 | angular_speed_steer : 0.5 29 | turn_angle : 1.5 30 | steering_timeout : 15.0 31 | # forward(旋回→直進→旋回の直進時の速度[m/s], 目標移動距離[m], タイムアウト[sec]) 32 | linear_vel_forward : 0.3 33 | step_forward_length : 1.0 34 | step_forward_timeout: 15.0 35 | -------------------------------------------------------------------------------- /stepback_and_steerturn_recovery/include/stepback_and_steerturn_recovery/stepback_and_steerturn_recovery.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | */ 30 | 31 | /** 32 | * \file 33 | * 34 | * Recovery behavior of step back and steer turning 35 | * 36 | * \author Masaru Morita 37 | */ 38 | 39 | #ifndef STEPBACK_AND_STEERTURN_RECOVERY_H 40 | #define STEPBACK_AND_STEERTURN_RECOVERY_H 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | namespace gm=geometry_msgs; 49 | namespace cmap=costmap_2d; 50 | namespace blp=base_local_planner; 51 | using std::vector; 52 | using std::max; 53 | 54 | namespace stepback_and_steerturn_recovery 55 | { 56 | 57 | /// Recovery behavior that takes a given twist and tries to execute it for up to 58 | /// d seconds, or until reaching an obstacle. 59 | class StepBackAndSteerTurnRecovery : public nav_core::RecoveryBehavior 60 | { 61 | public: 62 | 63 | /// Doesn't do anything: initialize is where the actual work happens 64 | StepBackAndSteerTurnRecovery(); 65 | 66 | ~StepBackAndSteerTurnRecovery(); 67 | 68 | /// Initialize the parameters of the behavior 69 | void initialize (std::string n, tf::TransformListener* tf, 70 | costmap_2d::Costmap2DROS* global_costmap, 71 | costmap_2d::Costmap2DROS* local_costmap); 72 | 73 | /// Run the behavior 74 | void runBehavior(); 75 | 76 | private: 77 | enum COSTMAP_SEARCH_MODE 78 | { 79 | FORWARD, 80 | FORWARD_LEFT, 81 | FORWARD_RIGHT, 82 | BACKWARD 83 | }; 84 | 85 | enum TURN_DIRECTION 86 | { 87 | LEFT, 88 | RIGHT, 89 | }; 90 | 91 | enum TURN_NO 92 | { 93 | FIRST_TURN = 0, 94 | SECOND_TURN = 1, 95 | }; 96 | static const int CNT_TURN = 2; 97 | 98 | gm::Twist TWIST_STOP; 99 | 100 | gm::Pose2D getCurrentLocalPose () const; 101 | gm::Twist scaleGivenAccelerationLimits (const gm::Twist& twist, const double time_remaining) const; 102 | gm::Pose2D getPoseToObstacle (const gm::Pose2D& current, const gm::Twist& twist) const; 103 | double normalizedPoseCost (const gm::Pose2D& pose) const; 104 | gm::Twist transformTwist (const gm::Pose2D& pose) const; 105 | void moveSpacifiedLength (const gm::Twist twist, const double length, const COSTMAP_SEARCH_MODE mode = FORWARD) const; 106 | double getCurrentDiff(const gm::Pose2D initialPose, const COSTMAP_SEARCH_MODE mode = FORWARD) const; 107 | double getCurrentDistDiff(const gm::Pose2D initialPose, const double distination, const COSTMAP_SEARCH_MODE mode = FORWARD) const; 108 | double getMinimalDistanceToObstacle(const COSTMAP_SEARCH_MODE mode) const; 109 | int determineTurnDirection(); 110 | double getDistBetweenTwoPoints(const gm::Pose2D pose1, const gm::Pose2D pose2) const; 111 | 112 | 113 | ros::NodeHandle nh_; 114 | costmap_2d::Costmap2DROS* global_costmap_; 115 | costmap_2d::Costmap2DROS* local_costmap_; 116 | costmap_2d::Costmap2D costmap_; // Copy of local_costmap_, used by world model 117 | std::string name_; 118 | tf::TransformListener* tf_; 119 | ros::Publisher cmd_vel_pub_; 120 | ros::Publisher recover_run_pub_; 121 | bool initialized_; 122 | 123 | // Memory owned by this object 124 | // Mutable because footprintCost is not declared const 125 | mutable base_local_planner::CostmapModel* world_model_; 126 | 127 | gm::Twist base_frame_twist_; 128 | 129 | double duration_; 130 | double linear_speed_limit_; 131 | double angular_speed_limit_; 132 | double linear_acceleration_limit_; 133 | double angular_acceleration_limit_; 134 | double controller_frequency_; 135 | double simulation_frequency_; 136 | double simulation_inc_; 137 | 138 | bool only_single_steering_; 139 | int trial_times_; 140 | double obstacle_patience_; 141 | double obstacle_check_frequency_; 142 | double sim_angle_resolution_; 143 | //-- back 144 | double linear_vel_back_; 145 | double step_back_length_; 146 | double step_back_timeout_; 147 | //-- steer 148 | double linear_vel_steer_; 149 | double angular_speed_steer_; 150 | double turn_angle_; 151 | double steering_timeout_; 152 | //-- forward 153 | double linear_vel_forward_; 154 | double step_forward_length_; 155 | double step_forward_timeout_; 156 | 157 | }; 158 | 159 | } // namespace stepback_and_steerturn_recovery 160 | 161 | #endif // include guard 162 | -------------------------------------------------------------------------------- /stepback_and_steerturn_recovery/package.xml: -------------------------------------------------------------------------------- 1 | 2 | stepback_and_steerturn_recovery 3 | 1.0.0 4 | 5 | 6 | This package provides a recovery behavior for the navigation stack which 7 | steps back and proceed with a specified steer angle 8 | 9 | 10 | CIR-KIT 11 | Masaru Morita 12 | Masaru Morita 13 | 14 | BSD 15 | https://github.com/CIR-KIT/steer_drive_ros.git 16 | https://github.com/CIR-KIT/steer_drive_ros/issues 17 | 18 | catkin 19 | roscpp 20 | tf 21 | costmap_2d 22 | nav_core 23 | pluginlib 24 | eigen 25 | cmake_modules 26 | base_local_planner 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /stepback_and_steerturn_recovery/path.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # include path を作成する 4 | include_path=$(pwd)/include 5 | 6 | # CPATHに追加 7 | export CPATH=${CPATH}:${include_path}:${gazebo_path}:${sdf_path} 8 | -------------------------------------------------------------------------------- /stepback_and_steerturn_recovery/stepback_and_steerturn_recovery_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Steps back and proceed with a specified steer angle. 5 | 6 | 7 | 8 | --------------------------------------------------------------------------------