├── ackermann_vehicle_gazebo ├── rosdoc.yaml ├── epydoc.config ├── models │ ├── warehouse │ │ ├── 1.jpg │ │ ├── asphalt.jpg │ │ ├── Wood_OSB.jpg │ │ ├── wall_texture.jpg │ │ ├── texture.tga.001.tga │ │ ├── warehouse_floor.jpg │ │ ├── 1513175627-picsay.jpg │ │ ├── 1513215312-picsay.jpg │ │ └── 1513215312-picsay.jpg.001.jpg │ └── checkerboard │ │ ├── checkerboard.png │ │ └── checkerboard.dae ├── CMakeLists.txt ├── launch │ ├── stereo_camera_calibration.launch │ └── ackermann_vehicle.launch ├── worlds │ ├── empty.world │ ├── checkerboard.world │ └── warehouse.world ├── package.xml ├── config │ ├── em_3905_ackermann_ctrlr_params.yaml │ └── em_3905_joint_ctrlr_params.yaml └── nodes │ └── ackermann_controller.py ├── README.md ├── ackermann_vehicle ├── CMakeLists.txt └── package.xml ├── ackermann_vehicle_description ├── CMakeLists.txt ├── launch │ ├── rviz.launch │ ├── ackermann_vehicle.launch │ └── ackermann_vehicle_rviz.launch ├── package.xml └── urdf │ ├── stereo_camera.urdf.xacro │ └── em_3905.urdf.xacro └── LICENSE.md /ackermann_vehicle_gazebo/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: epydoc 2 | config: epydoc.config 3 | -------------------------------------------------------------------------------- /ackermann_vehicle_gazebo/epydoc.config: -------------------------------------------------------------------------------- 1 | [epydoc] 2 | modules: nodes/*.py 3 | docformat: restructuredtext 4 | private: no 5 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ackermann_vehicle 2 | ================= 3 | 4 | ROS packages for simulating a vehicle with Ackermann steering 5 | -------------------------------------------------------------------------------- /ackermann_vehicle_gazebo/models/warehouse/1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gkouros/ackermann_vehicle/HEAD/ackermann_vehicle_gazebo/models/warehouse/1.jpg -------------------------------------------------------------------------------- /ackermann_vehicle/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ackermann_vehicle) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /ackermann_vehicle_gazebo/models/warehouse/asphalt.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gkouros/ackermann_vehicle/HEAD/ackermann_vehicle_gazebo/models/warehouse/asphalt.jpg -------------------------------------------------------------------------------- /ackermann_vehicle_gazebo/models/warehouse/Wood_OSB.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gkouros/ackermann_vehicle/HEAD/ackermann_vehicle_gazebo/models/warehouse/Wood_OSB.jpg -------------------------------------------------------------------------------- /ackermann_vehicle_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ackermann_vehicle_gazebo) 3 | find_package(catkin REQUIRED) 4 | catkin_package() 5 | -------------------------------------------------------------------------------- /ackermann_vehicle_gazebo/models/warehouse/wall_texture.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gkouros/ackermann_vehicle/HEAD/ackermann_vehicle_gazebo/models/warehouse/wall_texture.jpg -------------------------------------------------------------------------------- /ackermann_vehicle_gazebo/models/checkerboard/checkerboard.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gkouros/ackermann_vehicle/HEAD/ackermann_vehicle_gazebo/models/checkerboard/checkerboard.png -------------------------------------------------------------------------------- /ackermann_vehicle_gazebo/models/warehouse/texture.tga.001.tga: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gkouros/ackermann_vehicle/HEAD/ackermann_vehicle_gazebo/models/warehouse/texture.tga.001.tga -------------------------------------------------------------------------------- /ackermann_vehicle_gazebo/models/warehouse/warehouse_floor.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gkouros/ackermann_vehicle/HEAD/ackermann_vehicle_gazebo/models/warehouse/warehouse_floor.jpg -------------------------------------------------------------------------------- /ackermann_vehicle_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ackermann_vehicle_description) 3 | find_package(catkin REQUIRED) 4 | catkin_package() 5 | -------------------------------------------------------------------------------- /ackermann_vehicle_gazebo/models/warehouse/1513175627-picsay.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gkouros/ackermann_vehicle/HEAD/ackermann_vehicle_gazebo/models/warehouse/1513175627-picsay.jpg -------------------------------------------------------------------------------- /ackermann_vehicle_gazebo/models/warehouse/1513215312-picsay.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gkouros/ackermann_vehicle/HEAD/ackermann_vehicle_gazebo/models/warehouse/1513215312-picsay.jpg -------------------------------------------------------------------------------- /ackermann_vehicle_gazebo/models/warehouse/1513215312-picsay.jpg.001.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gkouros/ackermann_vehicle/HEAD/ackermann_vehicle_gazebo/models/warehouse/1513215312-picsay.jpg.001.jpg -------------------------------------------------------------------------------- /ackermann_vehicle_description/launch/rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /ackermann_vehicle_gazebo/launch/stereo_camera_calibration.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /ackermann_vehicle_gazebo/worlds/empty.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://sun 7 | 8 | 9 | 10 | 11 | model://ground_plane 12 | 13 | 14 | 15 | 1000.0 16 | 0.001 17 | 1 18 | 19 | 20 | quick 21 | 150 22 | 0 23 | 1.400000 24 | 1 25 | 26 | 27 | 0.00001 28 | 0.2 29 | 2000.000000 30 | 0.01000 31 | 32 | 33 | 34 | 35 | 36 | 0.4 0.4 0.4 1 37 | 0.7 0.7 0.7 1 38 | true 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /ackermann_vehicle_description/launch/ackermann_vehicle.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 30 | 31 | 33 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /ackermann_vehicle_description/launch/ackermann_vehicle_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /ackermann_vehicle/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ackermann_vehicle 4 | 0.1.0 5 | 6 | ackermann_vehicle is a group of packages for simulating a vehicle with 7 | Ackermann steering. 8 | 9 | 24 | 25 | Jim Rothrock 26 | 27 | Apache 2.0 28 | 29 | http://wiki.ros.org/ackermann_vehicle 30 | https://github.com/wunderkammer-laboratory/ackermann_vehicle/issues 31 | https://github.com/wunderkammer-laboratory/ackermann_vehicle.git 32 | 33 | Jim Rothrock 34 | 35 | catkin 36 | 37 | ackermann_vehicle_description 38 | ackermann_vehicle_gazebo 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /ackermann_vehicle_gazebo/worlds/checkerboard.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://sun 7 | 8 | 9 | 10 | 11 | model://ground_plane 12 | 13 | 14 | 15 | 1000.0 16 | 0.001 17 | 1 18 | 19 | 20 | quick 21 | 150 22 | 0 23 | 1.400000 24 | 1 25 | 26 | 27 | 0.00001 28 | 0.2 29 | 2000.000000 30 | 0.01000 31 | 32 | 33 | 34 | 35 | 36 | true 37 | 2 0 0.5 0 -1.5 0 38 | 39 | 40 | 41 | 42 | file://models/checkerboard/checkerboard.dae 43 | 1 1 1 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 1 1 1 1 52 | 0.3 0.3 0.3 1 53 | true 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /ackermann_vehicle_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ackermann_vehicle_description 4 | 0.1.0 5 | 6 | ackermann_vehicle_description contains the description (3D model, 7 | kinematics, mass, etc.) of a vehicle with Ackermann steering. 8 | 9 | 24 | 25 | Jim Rothrock 26 | 27 | Apache 2.0 28 | 29 | http://wiki.ros.org/ackermann_vehicle_description 30 | https://github.com/wunderkammer-laboratory/ackermann_vehicle/issues 31 | https://github.com/wunderkammer-laboratory/ackermann_vehicle.git 32 | 33 | Jim Rothrock 34 | 35 | catkin 36 | 37 | joint_state_publisher 38 | robot_state_publisher 39 | rviz 40 | xacro 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /ackermann_vehicle_gazebo/worlds/warehouse.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://sun 7 | 8 | 9 | 10 | 11 | model://ground_plane 12 | 13 | 14 | 15 | 1000.0 16 | 0.001 17 | 1 18 | 19 | 20 | quick 21 | 150 22 | 0 23 | 1.400000 24 | 1 25 | 26 | 27 | 0.00001 28 | 0.2 29 | 2000.000000 30 | 0.01000 31 | 32 | 33 | 34 | 35 | 36 | true 37 | 38 | 10 -8 0.1 0 0 3.141592654 39 | 40 | true 41 | 42 | 43 | file://models/warehouse/warehouse.dae 44 | 1 1 1 45 | 46 | 47 | 48 | 49 | 50 | 51 | file://models/warehouse/warehouse.dae 52 | 1 1 1 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 1 1 1 1 61 | 0.7 0.7 0.7 1 62 | true 63 | 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /ackermann_vehicle_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ackermann_vehicle_gazebo 4 | 0.1.0 5 | 6 | ackermann_vehicle_gazebo is used with Gazebo to simulate a vehicle with 7 | Ackermann steering. 8 | 9 | 24 | 25 | Jim Rothrock 26 | 27 | Apache 2.0 28 | 29 | http://wiki.ros.org/ackermann_vehicle_gazebo 30 | https://github.com/wunderkammer-laboratory/ackermann_vehicle/issues 31 | https://github.com/wunderkammer-laboratory/ackermann_vehicle.git 32 | 33 | Jim Rothrock 34 | 35 | catkin 36 | 37 | ackermann_msgs 38 | ackermann_vehicle_description 39 | controller_manager 40 | gazebo_ros 41 | gazebo_ros_control 42 | rospy 43 | std_msgs 44 | tf 45 | xacro 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /ackermann_vehicle_gazebo/config/em_3905_ackermann_ctrlr_params.yaml: -------------------------------------------------------------------------------- 1 | # em_3905_ackermann_ctrlr_params.yaml 2 | # 3 | # This file defines ackermann_controller parameter values used for simulating a 4 | # Traxxas(R) E-Maxx(R) #3905 RC (Radio Controlled) truck. 5 | # 6 | # Traxxas(R) and E-Maxx(R) are registered trademarks of Traxxas Management, 7 | # LLC. em_3905_ackermann_ctrlr_params.yaml was independently created by 8 | # Wunderkammer Laboratory, and neither em_3905_ackermann_ctrlr_params.yaml nor 9 | # Wunderkammer Laboratory is affiliated with, sponsored by, approved by, 10 | # or endorsed by Traxxas Management, LLC. All other trademarks and service 11 | # marks are the property of their respective owners. 12 | # 13 | # Copyright (c) 2013 Wunderkammer Laboratory 14 | # 15 | # Licensed under the Apache License, Version 2.0 (the "License"); 16 | # you may not use this file except in compliance with the License. 17 | # You may obtain a copy of the License at 18 | # 19 | # http://www.apache.org/licenses/LICENSE-2.0 20 | # 21 | # Unless required by applicable law or agreed to in writing, software 22 | # distributed under the License is distributed on an "AS IS" BASIS, 23 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 24 | # See the License for the specific language governing permissions and 25 | # limitations under the License. 26 | 27 | left_front_wheel: 28 | steering_link_name: left_front_wheel 29 | steering_controller_name: left_steering_ctrlr 30 | axle_controller_name: left_front_axle_ctrlr 31 | diameter: 0.14605 32 | 33 | right_front_wheel: 34 | steering_link_name: right_front_wheel 35 | steering_controller_name: right_steering_ctrlr 36 | axle_controller_name: right_front_axle_ctrlr 37 | diameter: 0.14605 38 | 39 | left_rear_wheel: 40 | link_name: left_rear_wheel 41 | axle_controller_name: left_rear_axle_ctrlr 42 | diameter: 0.14605 43 | 44 | right_rear_wheel: 45 | link_name: right_rear_wheel 46 | axle_controller_name: right_rear_axle_ctrlr 47 | diameter: 0.14605 48 | 49 | # The equilibrium position of each shock absorber is wheel_travel / 2. 50 | # wheel_travel, defined in em_3905.urdf.xacro, is 0.084 meters. 51 | 52 | shock_absorbers: 53 | - controller_name: left_front_shock_ctrlr 54 | equilibrium_position: 0.042 55 | - controller_name: right_front_shock_ctrlr 56 | equilibrium_position: 0.042 57 | - controller_name: left_rear_shock_ctrlr 58 | equilibrium_position: 0.042 59 | - controller_name: right_rear_shock_ctrlr 60 | equilibrium_position: 0.042 61 | -------------------------------------------------------------------------------- /ackermann_vehicle_gazebo/config/em_3905_joint_ctrlr_params.yaml: -------------------------------------------------------------------------------- 1 | # em_3905_joint_ctrlr_params.yaml 2 | # 3 | # This file defines joint controller parameter values used for simulating a 4 | # Traxxas(R) E-Maxx(R) #3905 RC (Radio Controlled) truck. 5 | # 6 | # Traxxas(R) and E-Maxx(R) are registered trademarks of Traxxas Management, 7 | # LLC. em_3905_joint_ctrlr_params.yaml was independently created by 8 | # Wunderkammer Laboratory, and neither em_3905_ros_ctrlr_params.yaml nor 9 | # Wunderkammer Laboratory is affiliated with, sponsored by, approved by, or 10 | # endorsed by Traxxas Management, LLC. All other trademarks and service marks 11 | # are the property of their respective owners. 12 | # 13 | # Copyright (c) 2013 Wunderkammer Laboratory 14 | # 15 | # Licensed under the Apache License, Version 2.0 (the "License"); 16 | # you may not use this file except in compliance with the License. 17 | # You may obtain a copy of the License at 18 | # 19 | # http://www.apache.org/licenses/LICENSE-2.0 20 | # 21 | # Unless required by applicable law or agreed to in writing, software 22 | # distributed under the License is distributed on an "AS IS" BASIS, 23 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 24 | # See the License for the specific language governing permissions and 25 | # limitations under the License. 26 | 27 | # Publish the joint states to joint_states. 28 | joint_state_ctrlr: 29 | type: joint_state_controller/JointStateController 30 | publish_rate: 30 31 | 32 | left_steering_ctrlr: 33 | joint: left_steering_joint 34 | type: effort_controllers/JointPositionController 35 | pid: {p: 4.0, i: 0.0, d: 1.0} 36 | right_steering_ctrlr: 37 | joint: right_steering_joint 38 | type: effort_controllers/JointPositionController 39 | pid: {p: 4.0, i: 0.0, d: 1.0} 40 | 41 | left_front_axle_ctrlr: 42 | joint: left_front_axle 43 | type: effort_controllers/JointVelocityController 44 | pid: {p: 1.5, i: 1.0, d: 0.0, i_clamp: 10.0} 45 | right_front_axle_ctrlr: 46 | joint: right_front_axle 47 | type: effort_controllers/JointVelocityController 48 | pid: {p: 1.5, i: 1.0, d: 0.0, i_clamp: 10.0} 49 | left_rear_axle_ctrlr: 50 | joint: left_rear_axle 51 | type: effort_controllers/JointVelocityController 52 | pid: {p: 1.5, i: 1.0, d: 0.0, i_clamp: 10.0} 53 | right_rear_axle_ctrlr: 54 | type: effort_controllers/JointVelocityController 55 | joint: right_rear_axle 56 | pid: {p: 1.5, i: 1.0, d: 0.0, i_clamp: 10.0} 57 | 58 | # The proportional gain of each shock absorber controller is 59 | # 2 * (shock_stroke * shock_spring_constant / wheel_travel). shock_stroke is 60 | # 0.028575 m. shock_spring_constant, an approximation of a Traxxas Ultra Shock 61 | # shock absorber spring's constant, is 437.817 N/m. wheel_travel, defined in 62 | # em_3905.urdf.xacro, is 0.084 m. 63 | # 64 | # The derivative gain of each shock absorber controller is 2 * shock_damping. 65 | # shock_damping, an approximation of the viscous damping coefficient of an 66 | # Ultra Shock shock absorber, is 36.2416 Ns/m 67 | # (i.e. sqrt(3 * shock_spring_constant)). 68 | 69 | left_front_shock_ctrlr: 70 | joint: left_front_shock 71 | type: effort_controllers/JointPositionController 72 | pid: {p: 297.8719, i: 0.0, d: 72.4832} 73 | right_front_shock_ctrlr: 74 | joint: right_front_shock 75 | type: effort_controllers/JointPositionController 76 | pid: {p: 297.8719, i: 0.0, d: 72.4832} 77 | left_rear_shock_ctrlr: 78 | joint: left_rear_shock 79 | type: effort_controllers/JointPositionController 80 | pid: {p: 297.8719, i: 0.0, d: 72.4832} 81 | right_rear_shock_ctrlr: 82 | joint: right_rear_shock 83 | type: effort_controllers/JointPositionController 84 | pid: {p: 297.8719, i: 0.0, d: 72.4832} 85 | -------------------------------------------------------------------------------- /ackermann_vehicle_gazebo/launch/ackermann_vehicle.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 72 | 73 | 75 | 77 | 78 | 79 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | -------------------------------------------------------------------------------- /ackermann_vehicle_description/urdf/stereo_camera.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 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 | Gazebo/LightGray 60 | 61 | 62 | Gazebo/Black 63 | 64 | 65 | Gazebo/Black 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 15.0 99 | 100 | 0 0.06 0 0 0 0 101 | 1.570796327 102 | 103 | 640 104 | 480 105 | R8G8B8 106 | 107 | 108 | 0.05 109 | 30 110 | 111 | 112 | -0.25 113 | 0.12 114 | 0.0 115 | -0.00028 116 | -0.00005 117 |
0.5 0.5
118 | 119 | 120 | 121 | 0 -0.06 0 0 0 0 122 | 1.570796327 123 | 124 | 640 125 | 480 126 | R8G8B8 127 | 128 | 129 | 0.05 130 | 30 131 | 132 | 133 | -0.25 134 | 0.12 135 | 0.0 136 | -0.00028 137 | -0.00005 138 |
0.5 0.5
139 | 140 | 141 | 142 | true 143 | 15.0 144 | stereo_camera 145 | image_raw 146 | camera_info 147 | stereo_left_camera_frame 148 | stereo_right_camera_frame 149 | 0.12 150 | 151 | 152 | 153 | 154 | 155 | -0.0 156 | 0.0 157 | 0.0 158 | 0.0 159 | 0.0 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, and 10 | distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by the copyright 13 | owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all other entities 16 | that control, are controlled by, or are under common control with that entity. 17 | For the purposes of this definition, "control" means (i) the power, direct or 18 | indirect, to cause the direction or management of such entity, whether by 19 | contract or otherwise, or (ii) ownership of fifty percent (50%) or more of the 20 | outstanding shares, or (iii) beneficial ownership of such entity. 21 | 22 | "You" (or "Your") shall mean an individual or Legal Entity exercising 23 | permissions granted by this License. 24 | 25 | "Source" form shall mean the preferred form for making modifications, including 26 | but not limited to software source code, documentation source, and configuration 27 | files. 28 | 29 | "Object" form shall mean any form resulting from mechanical transformation or 30 | translation of a Source form, including but not limited to compiled object code, 31 | generated documentation, and conversions to other media types. 32 | 33 | "Work" shall mean the work of authorship, whether in Source or Object form, made 34 | available under the License, as indicated by a copyright notice that is included 35 | in or attached to the work (an example is provided in the Appendix below). 36 | 37 | "Derivative Works" shall mean any work, whether in Source or Object form, that 38 | is based on (or derived from) the Work and for which the editorial revisions, 39 | annotations, elaborations, or other modifications represent, as a whole, an 40 | original work of authorship. For the purposes of this License, Derivative Works 41 | shall not include works that remain separable from, or merely link (or bind by 42 | name) to the interfaces of, the Work and Derivative Works thereof. 43 | 44 | "Contribution" shall mean any work of authorship, including the original version 45 | of the Work and any modifications or additions to that Work or Derivative Works 46 | thereof, that is intentionally submitted to Licensor for inclusion in the Work 47 | by the copyright owner or by an individual or Legal Entity authorized to submit 48 | on behalf of the copyright owner. For the purposes of this definition, 49 | "submitted" means any form of electronic, verbal, or written communication sent 50 | to the Licensor or its representatives, including but not limited to 51 | communication on electronic mailing lists, source code control systems, and 52 | issue tracking systems that are managed by, or on behalf of, the Licensor for 53 | the purpose of discussing and improving the Work, but excluding communication 54 | that is conspicuously marked or otherwise designated in writing by the copyright 55 | owner as "Not a Contribution." 56 | 57 | "Contributor" shall mean Licensor and any individual or Legal Entity on behalf 58 | of whom a Contribution has been received by Licensor and subsequently 59 | incorporated within the Work. 60 | 61 | 2. Grant of Copyright License. 62 | 63 | Subject to the terms and conditions of this License, each Contributor hereby 64 | grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, 65 | irrevocable copyright license to reproduce, prepare Derivative Works of, 66 | publicly display, publicly perform, sublicense, and distribute the Work and such 67 | Derivative Works in Source or Object form. 68 | 69 | 3. Grant of Patent License. 70 | 71 | Subject to the terms and conditions of this License, each Contributor hereby 72 | grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, 73 | irrevocable (except as stated in this section) patent license to make, have 74 | made, use, offer to sell, sell, import, and otherwise transfer the Work, where 75 | such license applies only to those patent claims licensable by such Contributor 76 | that are necessarily infringed by their Contribution(s) alone or by combination 77 | of their Contribution(s) with the Work to which such Contribution(s) was 78 | submitted. If You institute patent litigation against any entity (including a 79 | cross-claim or counterclaim in a lawsuit) alleging that the Work or a 80 | Contribution incorporated within the Work constitutes direct or contributory 81 | patent infringement, then any patent licenses granted to You under this License 82 | for that Work shall terminate as of the date such litigation is filed. 83 | 84 | 4. Redistribution. 85 | 86 | You may reproduce and distribute copies of the Work or Derivative Works thereof 87 | in any medium, with or without modifications, and in Source or Object form, 88 | provided that You meet the following conditions: 89 | 90 | You must give any other recipients of the Work or Derivative Works a copy of 91 | this License; and 92 | You must cause any modified files to carry prominent notices stating that You 93 | changed the files; and 94 | You must retain, in the Source form of any Derivative Works that You distribute, 95 | all copyright, patent, trademark, and attribution notices from the Source form 96 | of the Work, excluding those notices that do not pertain to any part of the 97 | Derivative Works; and 98 | If the Work includes a "NOTICE" text file as part of its distribution, then any 99 | Derivative Works that You distribute must include a readable copy of the 100 | attribution notices contained within such NOTICE file, excluding those notices 101 | that do not pertain to any part of the Derivative Works, in at least one of the 102 | following places: within a NOTICE text file distributed as part of the 103 | Derivative Works; within the Source form or documentation, if provided along 104 | with the Derivative Works; or, within a display generated by the Derivative 105 | Works, if and wherever such third-party notices normally appear. The contents of 106 | the NOTICE file are for informational purposes only and do not modify the 107 | License. You may add Your own attribution notices within Derivative Works that 108 | You distribute, alongside or as an addendum to the NOTICE text from the Work, 109 | provided that such additional attribution notices cannot be construed as 110 | modifying the License. 111 | You may add Your own copyright statement to Your modifications and may provide 112 | additional or different license terms and conditions for use, reproduction, or 113 | distribution of Your modifications, or for any such Derivative Works as a whole, 114 | provided Your use, reproduction, and distribution of the Work otherwise complies 115 | with the conditions stated in this License. 116 | 117 | 5. Submission of Contributions. 118 | 119 | Unless You explicitly state otherwise, any Contribution intentionally submitted 120 | for inclusion in the Work by You to the Licensor shall be under the terms and 121 | conditions of this License, without any additional terms or conditions. 122 | Notwithstanding the above, nothing herein shall supersede or modify the terms of 123 | any separate license agreement you may have executed with Licensor regarding 124 | such Contributions. 125 | 126 | 6. Trademarks. 127 | 128 | This License does not grant permission to use the trade names, trademarks, 129 | service marks, or product names of the Licensor, except as required for 130 | reasonable and customary use in describing the origin of the Work and 131 | reproducing the content of the NOTICE file. 132 | 133 | 7. Disclaimer of Warranty. 134 | 135 | Unless required by applicable law or agreed to in writing, Licensor provides the 136 | Work (and each Contributor provides its Contributions) on an "AS IS" BASIS, 137 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied, 138 | including, without limitation, any warranties or conditions of TITLE, 139 | NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A PARTICULAR PURPOSE. You are 140 | solely responsible for determining the appropriateness of using or 141 | redistributing the Work and assume any risks associated with Your exercise of 142 | permissions under this License. 143 | 144 | 8. Limitation of Liability. 145 | 146 | In no event and under no legal theory, whether in tort (including negligence), 147 | contract, or otherwise, unless required by applicable law (such as deliberate 148 | and grossly negligent acts) or agreed to in writing, shall any Contributor be 149 | liable to You for damages, including any direct, indirect, special, incidental, 150 | or consequential damages of any character arising as a result of this License or 151 | out of the use or inability to use the Work (including but not limited to 152 | damages for loss of goodwill, work stoppage, computer failure or malfunction, or 153 | any and all other commercial damages or losses), even if such Contributor has 154 | been advised of the possibility of such damages. 155 | 156 | 9. Accepting Warranty or Additional Liability. 157 | 158 | While redistributing the Work or Derivative Works thereof, You may choose to 159 | offer, and charge a fee for, acceptance of support, warranty, indemnity, or 160 | other liability obligations and/or rights consistent with this License. However, 161 | in accepting such obligations, You may act only on Your own behalf and on Your 162 | sole responsibility, not on behalf of any other Contributor, and only if You 163 | agree to indemnify, defend, and hold each Contributor harmless for any liability 164 | incurred by, or claims asserted against, such Contributor by reason of your 165 | accepting any such warranty or additional liability. 166 | 167 | END OF TERMS AND CONDITIONS 168 | 169 | APPENDIX: How to apply the Apache License to your work 170 | 171 | To apply the Apache License to your work, attach the following boilerplate 172 | notice, with the fields enclosed by brackets "[]" replaced with your own 173 | identifying information. (Don't include the brackets!) The text should be 174 | enclosed in the appropriate comment syntax for the file format. We also 175 | recommend that a file or class name and description of purpose be included on 176 | the same "printed page" as the copyright notice for easier identification within 177 | third-party archives. 178 | 179 | Copyright [yyyy] [name of copyright owner] 180 | 181 | Licensed under the Apache License, Version 2.0 (the "License"); 182 | you may not use this file except in compliance with the License. 183 | You may obtain a copy of the License at 184 | 185 | http://www.apache.org/licenses/LICENSE-2.0 186 | 187 | Unless required by applicable law or agreed to in writing, software 188 | distributed under the License is distributed on an "AS IS" BASIS, 189 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 190 | See the License for the specific language governing permissions and 191 | limitations under the License. 192 | -------------------------------------------------------------------------------- /ackermann_vehicle_gazebo/models/checkerboard/checkerboard.dae: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Blender User 6 | Blender 2.79.0 7 | 8 | 2019-02-18T15:50:57 9 | 2019-02-18T15:50:57 10 | 11 | Z_UP 12 | 13 | 14 | 15 | 16 | 17 | 18 | 49.13434 19 | 1.777778 20 | 0.1 21 | 100 22 | 23 | 24 | 25 | 26 | 27 | 0 28 | 0 29 | 0 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 1 1 1 39 | 1 40 | 0 41 | 0.00111109 42 | 43 | 44 | 45 | 46 | 0 47 | 0 48 | 8192 49 | 1 50 | 1 51 | 1 52 | 1 53 | 0 54 | 0 55 | 0 56 | 1 57 | 29.99998 58 | 75 59 | 0.15 60 | 1 61 | 0 62 | 1 63 | 2 64 | 1.000799 65 | 30.002 66 | 1 67 | 3 68 | 0.04999995 69 | 2880 70 | 3 71 | 1 72 | 0 73 | 0 74 | 2 75 | 1 76 | 1 77 | 1 78 | 0 79 | 1 80 | 0.1 81 | 0.1 82 | 1 83 | 0.000999987 84 | 1 85 | 0 86 | 0 87 | 1 88 | 1 89 | 1 90 | 1 91 | 1 92 | 1 93 | 1 94 | 2 95 | 1 96 | 1 97 | 1 98 | 1 99 | 0 100 | 101 | 102 | 103 | 104 | 105 | 106 | checkerboard.png 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | checkerboard_png 115 | 116 | 117 | 118 | 119 | checkerboard_png-surface 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 1 1 1 1 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 0.4499998 0.5499997 -0.004999995 0.4499998 -0.5499998 -0.004999995 -0.4499998 -0.5499997 -0.004999995 -0.4499996 0.55 -0.004999995 0.45 0.5499995 0.004999995 0.4499995 -0.5500001 0.004999995 -0.4499999 -0.5499996 0.004999995 -0.4499998 0.5499998 0.004999995 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 0 0 -1 0 0 1 1 0 -2.08616e-5 -1.32455e-7 -1 -1.98682e-5 -1 1.69331e-7 -1.19209e-5 3.31137e-7 1 1.98682e-5 0 0 -1 0 0 1 1 -4.23329e-7 2.98023e-5 -5.29819e-7 -1 4.30478e-6 -1 0 -8.9407e-6 2.6491e-7 1 2.11928e-5 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 8.07303e-5 0.6508023 0.9999193 8.0765e-5 0.9999195 0.6508021 9.85819e-5 9.85819e-5 0.9999015 0.9721289 9.86668e-5 0.9721292 0.9836065 0.6111111 0.989071 1.32455e-7 0.989071 0.6111112 0.9945355 0.9999998 1 0.5000001 1 1 0.9945355 0.6111111 0.989071 0 0.9945355 0 1 1.32455e-7 0.9945355 0.5000001 0.9945355 0 8.07303e-5 0.6508023 8.06608e-5 8.06608e-5 0.9999193 8.0765e-5 9.85819e-5 9.85819e-5 0.9999013 9.86668e-5 0.9999015 0.9721289 0.9836065 0.6111111 0.9836065 0 0.989071 1.32455e-7 0.9945355 0.9999998 0.9945355 0.5000001 1 0.5000001 0.9945355 0.6111111 0.989071 0.6111111 0.989071 0 1 1.32455e-7 1 0.5 0.9945355 0.5000001 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 |

4 1 3 6 1 4 5 1 5 4 7 21 7 7 22 6 7 23

180 | 181 | 182 | 183 | 184 | 185 |

1 0 0 3 0 1 0 0 2 4 2 6 1 2 7 0 2 8 5 3 9 2 3 10 1 3 11 2 4 12 7 4 13 3 4 14 0 5 15 7 5 16 4 5 17 1 6 18 2 6 19 3 6 20 4 8 24 5 8 25 1 8 26 5 9 27 6 9 28 2 9 29 2 10 30 6 10 31 7 10 32 0 11 33 3 11 34 7 11 35

186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 0.6859207 -0.3240135 0.6515582 7.481132 0.7276763 0.3054208 -0.6141704 -6.50764 0 0.8953956 0.4452714 5.343665 0 0 0 1 195 | 196 | 197 | 198 | -0.2908646 -0.7711008 0.5663932 4.076245 0.9551712 -0.1998834 0.2183912 1.005454 -0.05518906 0.6045247 0.7946723 5.903862 0 0 0 1 199 | 200 | 201 | 202 | 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | -------------------------------------------------------------------------------- /ackermann_vehicle_description/urdf/em_3905.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 50 | 51 | 52 | 53 | 54 | 55 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 93 | 94 | 95 | 96 | 110 | 111 | 112 | 113 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 143 | 145 | 146 | 147 | 148 | 153 | 154 | 155 | 156 | 159 | 161 | 162 | 163 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 214 | 215 | 216 | 217 | 218 | 219 | Gazebo/Blue 220 | 221 | 222 | 223 | 224 | 226 | 227 | 228 | 229 | 230 | 233 | 234 | 236 | 237 | 238 | transmission_interface/SimpleTransmission 239 | 240 | hardware_interface/EffortJointInterface 241 | 242 | 243 | 1 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | transmission_interface/SimpleTransmission 265 | 266 | hardware_interface/EffortJointInterface 267 | 268 | 269 | 1 270 | 271 | 272 | 273 | 274 | 275 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 290 | 291 | 292 | 293 | 294 | 295 | 298 | 299 | 300 | 301 | Gazebo/Black 302 | 303 | 304 | 305 | 306 | 308 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 322 | 323 | 324 | transmission_interface/SimpleTransmission 325 | 326 | hardware_interface/EffortJointInterface 327 | 328 | 329 | 1 330 | 331 | 332 | 333 | 335 | 336 | 337 | 338 | 340 | 343 | 345 | 346 | 347 | 349 | 350 | 351 | 352 | 353 | 354 | 355 | 356 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | 365 | 366 | 367 | 368 | 371 | 372 | 373 | 374 | 375 | Gazebo/White 376 | 377 | 378 | 379 | 380 | 381 | 382 | 383 | 384 | 385 | 386 | 387 | 388 | 390 | 392 | 394 | 396 | 397 | 398 | 399 | 400 | 401 | 402 | 403 | 404 | 405 | 406 | 407 | 408 | Gazebo/Blue 409 | 410 | 411 | 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | 422 | 423 | map 424 | base_link 425 | odom_truth 426 | 30.0 427 | 428 | 429 | 430 | 431 | -------------------------------------------------------------------------------- /ackermann_vehicle_gazebo/nodes/ackermann_controller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ackermann_controller.py 4 | 5 | Control the wheels of a vehicle with Ackermann steering. 6 | 7 | Subscribed Topics: 8 | ackermann_cmd (ackermann_msgs/AckermannDriveStamped) 9 | Ackermann command. It contains the vehicle's desired speed and steering 10 | angle. 11 | 12 | Published Topics: 13 | /command (std_msgs/Float64) 14 | Command for the left steering controller. 15 | /command (std_msgs/Float64) 16 | Command for the right steering controller. 17 | /command (std_msgs/Float64) 18 | Command for the left front axle controller. 19 | /command (std_msgs/Float64) 20 | Command for the right front axle controller. 21 | /command (std_msgs/Float64) 22 | Command for the left rear axle controller. 23 | /command (std_msgs/Float64) 24 | Command for the right rear axle controller. 25 | /command (std_msgs/Float64) 26 | One of these topics exists for each shock absorber. They are latched 27 | topics. 28 | 29 | Services Called: 30 | controller_manager/list_controllers (controller_manager_msgs/ 31 | ListControllers) 32 | List the states of the controllers. 33 | 34 | Parameters: 35 | ~left_front_wheel/steering_link_name (string, default: left_steering_link) 36 | ~right_front_wheel/steering_link_name (string, 37 | default: right_steering_link) 38 | Names of links that have origins coincident with the origins of the 39 | left and right steering joints, respectively. The steering links are 40 | used to compute the distance between the steering joints, as well as 41 | the vehicle's wheelbase. 42 | 43 | ~left_front_wheel/steering_controller_name (string, default: 44 | left_steering_controller) 45 | ~right_front_wheel/steering_controller_name (string, default: 46 | right_steering_controller) 47 | Steering controller names. 48 | 49 | ~left_rear_wheel/link_name (string, default: left_wheel) 50 | ~right_rear_wheel/link_name (string, default: right_wheel) 51 | Names of links that have origins coincident with the centers of the 52 | left and right wheels, respectively. The rear wheel links are used to 53 | compute the vehicle's wheelbase. 54 | 55 | ~left_front_wheel/axle_controller_name (string) 56 | ~right_front_wheel/axle_controller_name 57 | ~left_rear_wheel/axle_controller_name 58 | ~right_rear_wheel/axle_controller_name 59 | Axle controller names. If no controller name is specified for an axle, 60 | that axle will not have a controller. This allows the control of 61 | front-wheel, rear-wheel, and four-wheel drive vehicles. 62 | 63 | ~left_front_wheel/diameter (float, default: 1.0) 64 | ~right_front_wheel/diameter 65 | ~left_rear_wheel/diameter 66 | ~right_rear_wheel/diameter 67 | Wheel diameters. Each diameter must be greater than zero. Unit: meter. 68 | 69 | ~shock_absorbers (sequence of mappings, default: empty) 70 | Zero or more shock absorbers. 71 | 72 | Key-Value Pairs: 73 | 74 | controller_name (string) 75 | Controller name. 76 | equilibrium_position (float, default: 0.0) 77 | Equilibrium position. Unit: meter. 78 | 79 | ~cmd_timeout (float, default: 0.5) 80 | If ~cmd_timeout is greater than zero and this node does not receive a 81 | command for more than ~cmd_timeout seconds, vehicle motion is paused 82 | until a command is received. If ~cmd_timeout is less than or equal to 83 | zero, the command timeout is disabled. 84 | ~publishing_frequency (float, default: 30.0) 85 | Joint command publishing frequency. It must be greater than zero. 86 | Unit: hertz. 87 | 88 | Required tf Transforms: 89 | <~left_front_wheel/steering_link_name> to <~right_rear_wheel/link_name> 90 | Specifies the position of the left front wheel's steering link in the 91 | right rear wheel's frame. 92 | <~right_front_wheel/steering_link_name> to <~right_rear_wheel/link_name> 93 | Specifies the position of the right front wheel's steering link in the 94 | right rear wheel's frame. 95 | <~left_rear_wheel/link_name> to <~right_rear_wheel/link_name> 96 | Specifies the position of the left rear wheel in the right rear 97 | wheel's frame. 98 | 99 | Copyright (c) 2013 Wunderkammer Laboratory 100 | 101 | Licensed under the Apache License, Version 2.0 (the "License"); 102 | you may not use this file except in compliance with the License. 103 | You may obtain a copy of the License at 104 | 105 | http://www.apache.org/licenses/LICENSE-2.0 106 | 107 | Unless required by applicable law or agreed to in writing, software 108 | distributed under the License is distributed on an "AS IS" BASIS, 109 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 110 | See the License for the specific language governing permissions and 111 | limitations under the License. 112 | """ 113 | 114 | import math 115 | import numpy 116 | import threading 117 | 118 | from math import pi 119 | 120 | import rospy 121 | import tf 122 | 123 | from ackermann_msgs.msg import AckermannDriveStamped 124 | from std_msgs.msg import Float64 125 | from controller_manager_msgs.srv import ListControllers 126 | 127 | 128 | class _AckermannCtrlr(object): 129 | """Ackermann controller 130 | 131 | An object of class _AckermannCtrlr is a node that controls the wheels of a 132 | vehicle with Ackermann steering. 133 | """ 134 | 135 | def __init__(self): 136 | """Initialize this _AckermannCtrlr.""" 137 | 138 | rospy.init_node("ackermann_controller") 139 | 140 | # Parameters 141 | 142 | # Wheels 143 | (left_steer_link_name, left_steer_ctrlr_name, 144 | left_front_axle_ctrlr_name, self._left_front_inv_circ) = \ 145 | self._get_front_wheel_params("left") 146 | (right_steer_link_name, right_steer_ctrlr_name, 147 | right_front_axle_ctrlr_name, self._right_front_inv_circ) = \ 148 | self._get_front_wheel_params("right") 149 | (left_rear_link_name, left_rear_axle_ctrlr_name, 150 | self._left_rear_inv_circ) = \ 151 | self._get_rear_wheel_params("left") 152 | (self._right_rear_link_name, right_rear_axle_ctrlr_name, 153 | self._right_rear_inv_circ) = \ 154 | self._get_rear_wheel_params("right") 155 | 156 | list_ctrlrs = rospy.ServiceProxy("controller_manager/list_controllers", 157 | ListControllers) 158 | list_ctrlrs.wait_for_service() 159 | 160 | # Shock absorbers 161 | shock_param_list = rospy.get_param("~shock_absorbers", []) 162 | self._shock_pubs = [] 163 | try: 164 | for shock_params in shock_param_list: 165 | try: 166 | ctrlr_name = shock_params["controller_name"] 167 | try: 168 | eq_pos = shock_params["equilibrium_position"] 169 | except: 170 | eq_pos = self._DEF_EQ_POS 171 | eq_pos = float(eq_pos) 172 | except: 173 | rospy.logwarn("An invalid parameter was specified for a " 174 | "shock absorber. The shock absorber will " 175 | "not be used.") 176 | continue 177 | 178 | pub = rospy.Publisher(ctrlr_name + "/command", Float64, queue_size=10, 179 | latch=True) 180 | _wait_for_ctrlr(list_ctrlrs, ctrlr_name) 181 | pub.publish(eq_pos) 182 | self._shock_pubs.append(pub) 183 | except: 184 | rospy.logwarn("The specified list of shock absorbers is invalid. " 185 | "No shock absorbers will be used.") 186 | 187 | # Command timeout 188 | try: 189 | self._cmd_timeout = float(rospy.get_param("~cmd_timeout", 190 | self._DEF_CMD_TIMEOUT)) 191 | except: 192 | rospy.logwarn("The specified command timeout value is invalid. " 193 | "The default timeout value will be used instead.") 194 | self._cmd_timeout = self._DEF_CMD_TIMEOUT 195 | 196 | # Publishing frequency 197 | try: 198 | pub_freq = float(rospy.get_param("~publishing_frequency", 199 | self._DEF_PUB_FREQ)) 200 | if pub_freq <= 0.0: 201 | raise ValueError() 202 | except: 203 | rospy.logwarn("The specified publishing frequency is invalid. " 204 | "The default frequency will be used instead.") 205 | pub_freq = self._DEF_PUB_FREQ 206 | self._sleep_timer = rospy.Rate(pub_freq) 207 | 208 | # _last_cmd_time is the time at which the most recent Ackermann 209 | # driving command was received. 210 | self._last_cmd_time = rospy.get_time() 211 | 212 | # _ackermann_cmd_lock is used to control access to _steer_ang, 213 | # _steer_ang_vel, _speed, _accel, and _jerk. 214 | self._ackermann_cmd_lock = threading.Lock() 215 | self._steer_ang = 0.0 # Steering angle 216 | self._steer_ang_vel = 0.0 # Steering angle velocity 217 | self._speed = 0.0 218 | self._accel = 0.0 # Acceleration 219 | self._jerk = 0.0 220 | 221 | self._last_steer_ang = 0.0 # Last steering angle 222 | self._theta_left = 0.0 # Left steering joint angle 223 | self._theta_right = 0.0 # Right steering joint angle 224 | 225 | self._last_speed = 0.0 226 | self._last_accel_limit = 0.0 # Last acceleration limit 227 | # Axle angular velocities 228 | self._left_front_ang_vel = 0.0 229 | self._right_front_ang_vel = 0.0 230 | self._left_rear_ang_vel = 0.0 231 | self._right_rear_ang_vel = 0.0 232 | 233 | # _joint_dist_div_2 is the distance between the steering joints, 234 | # divided by two. 235 | tfl = tf.TransformListener() 236 | ls_pos = self._get_link_pos(tfl, left_steer_link_name) 237 | rs_pos = self._get_link_pos(tfl, right_steer_link_name) 238 | self._joint_dist_div_2 = numpy.linalg.norm(ls_pos - rs_pos) / 2 239 | lrw_pos = self._get_link_pos(tfl, left_rear_link_name) 240 | rrw_pos = numpy.array([0.0] * 3) 241 | front_cent_pos = (ls_pos + rs_pos) / 2 # Front center position 242 | rear_cent_pos = (lrw_pos + rrw_pos) / 2 # Rear center position 243 | self._wheelbase = numpy.linalg.norm(front_cent_pos - rear_cent_pos) 244 | self._inv_wheelbase = 1 / self._wheelbase # Inverse of _wheelbase 245 | self._wheelbase_sqr = self._wheelbase ** 2 246 | 247 | # Publishers and subscribers 248 | 249 | self._left_steer_cmd_pub = \ 250 | _create_cmd_pub(list_ctrlrs, left_steer_ctrlr_name) 251 | self._right_steer_cmd_pub = \ 252 | _create_cmd_pub(list_ctrlrs, right_steer_ctrlr_name) 253 | 254 | self._left_front_axle_cmd_pub = \ 255 | _create_axle_cmd_pub(list_ctrlrs, left_front_axle_ctrlr_name) 256 | self._right_front_axle_cmd_pub = \ 257 | _create_axle_cmd_pub(list_ctrlrs, right_front_axle_ctrlr_name) 258 | self._left_rear_axle_cmd_pub = \ 259 | _create_axle_cmd_pub(list_ctrlrs, left_rear_axle_ctrlr_name) 260 | self._right_rear_axle_cmd_pub = \ 261 | _create_axle_cmd_pub(list_ctrlrs, right_rear_axle_ctrlr_name) 262 | 263 | self._ackermann_cmd_sub = \ 264 | rospy.Subscriber("ackermann_cmd", AckermannDriveStamped, 265 | self.ackermann_cmd_cb, queue_size=1) 266 | 267 | def spin(self): 268 | """Control the vehicle.""" 269 | 270 | last_time = rospy.get_time() 271 | 272 | while not rospy.is_shutdown(): 273 | t = rospy.get_time() 274 | delta_t = t - last_time 275 | last_time = t 276 | 277 | if (self._cmd_timeout > 0.0 and 278 | t - self._last_cmd_time > self._cmd_timeout): 279 | # Too much time has elapsed since the last command. Stop the 280 | # vehicle. 281 | steer_ang_changed, center_y = \ 282 | self._ctrl_steering(self._last_steer_ang, 0.0, 0.001) 283 | self._ctrl_axles(0.0, 0.0, 0.0, 0.001, steer_ang_changed, 284 | center_y) 285 | elif delta_t > 0.0: 286 | with self._ackermann_cmd_lock: 287 | steer_ang = self._steer_ang 288 | steer_ang_vel = self._steer_ang_vel 289 | speed = self._speed 290 | accel = self._accel 291 | jerk = self._jerk 292 | steer_ang_changed, center_y = \ 293 | self._ctrl_steering(steer_ang, steer_ang_vel, delta_t) 294 | self._ctrl_axles(speed, accel, jerk, delta_t, 295 | steer_ang_changed, center_y) 296 | 297 | # Publish the steering and axle joint commands. 298 | self._left_steer_cmd_pub.publish(self._theta_left) 299 | self._right_steer_cmd_pub.publish(self._theta_right) 300 | if self._left_front_axle_cmd_pub: 301 | self._left_front_axle_cmd_pub.publish(self._left_front_ang_vel) 302 | if self._right_front_axle_cmd_pub: 303 | self._right_front_axle_cmd_pub.\ 304 | publish(self._right_front_ang_vel) 305 | if self._left_rear_axle_cmd_pub: 306 | self._left_rear_axle_cmd_pub.publish(self._left_rear_ang_vel) 307 | if self._right_rear_axle_cmd_pub: 308 | self._right_rear_axle_cmd_pub.publish(self._right_rear_ang_vel) 309 | 310 | self._sleep_timer.sleep() 311 | 312 | def ackermann_cmd_cb(self, ackermann_cmd): 313 | """Ackermann driving command callback 314 | 315 | :Parameters: 316 | ackermann_cmd : ackermann_msgs.msg.AckermannDriveStamped 317 | Ackermann driving command. 318 | """ 319 | self._last_cmd_time = rospy.get_time() 320 | with self._ackermann_cmd_lock: 321 | self._steer_ang = ackermann_cmd.drive.steering_angle 322 | self._steer_ang_vel = ackermann_cmd.drive.steering_angle_velocity 323 | self._speed = ackermann_cmd.drive.speed 324 | self._accel = ackermann_cmd.drive.acceleration 325 | self._jerk = ackermann_cmd.drive.jerk 326 | 327 | def _get_front_wheel_params(self, side): 328 | # Get front wheel parameters. Return a tuple containing the steering 329 | # link name, steering controller name, axle controller name (or None), 330 | # and inverse of the circumference. 331 | 332 | prefix = "~" + side + "_front_wheel/" 333 | steer_link_name = rospy.get_param(prefix + "steering_link_name", 334 | side + "_steering_link") 335 | steer_ctrlr_name = rospy.get_param(prefix + "steering_controller_name", 336 | side + "_steering_controller") 337 | axle_ctrlr_name, inv_circ = self._get_common_wheel_params(prefix) 338 | return steer_link_name, steer_ctrlr_name, axle_ctrlr_name, inv_circ 339 | 340 | def _get_rear_wheel_params(self, side): 341 | # Get rear wheel parameters. Return a tuple containing the link name, 342 | # axle controller name, and inverse of the circumference. 343 | 344 | prefix = "~" + side + "_rear_wheel/" 345 | link_name = rospy.get_param(prefix + "link_name", side + "_wheel") 346 | axle_ctrlr_name, inv_circ = self._get_common_wheel_params(prefix) 347 | return link_name, axle_ctrlr_name, inv_circ 348 | 349 | def _get_common_wheel_params(self, prefix): 350 | # Get parameters used by the front and rear wheels. Return a tuple 351 | # containing the axle controller name (or None) and the inverse of the 352 | # circumference. 353 | 354 | axle_ctrlr_name = rospy.get_param(prefix + "axle_controller_name", 355 | None) 356 | 357 | try: 358 | dia = float(rospy.get_param(prefix + "diameter", 359 | self._DEF_WHEEL_DIA)) 360 | if dia <= 0.0: 361 | raise ValueError() 362 | except: 363 | rospy.logwarn("The specified wheel diameter is invalid. " 364 | "The default diameter will be used instead.") 365 | dia = self._DEF_WHEEL_DIA 366 | 367 | return axle_ctrlr_name, 1 / (pi * dia) 368 | 369 | def _get_link_pos(self, tfl, link): 370 | # Return the position of the specified link, relative to the right 371 | # rear wheel link. 372 | 373 | while True: 374 | try: 375 | trans, not_used = \ 376 | tfl.lookupTransform(self._right_rear_link_name, link, 377 | rospy.Time(0)) 378 | return numpy.array(trans) 379 | except: 380 | pass 381 | 382 | def _ctrl_steering(self, steer_ang, steer_ang_vel_limit, delta_t): 383 | # Control the steering joints. 384 | 385 | # Compute theta, the virtual front wheel's desired steering angle. 386 | if steer_ang_vel_limit > 0.0: 387 | # Limit the steering velocity. 388 | ang_vel = (steer_ang - self._last_steer_ang) / delta_t 389 | ang_vel = max(-steer_ang_vel_limit, 390 | min(ang_vel, steer_ang_vel_limit)) 391 | theta = self._last_steer_ang + ang_vel * delta_t 392 | else: 393 | theta = steer_ang 394 | 395 | # Compute the desired steering angles for the left and right front 396 | # wheels. 397 | center_y = self._wheelbase * math.tan((pi / 2) - theta) 398 | steer_ang_changed = theta != self._last_steer_ang 399 | if steer_ang_changed: 400 | self._last_steer_ang = theta 401 | self._theta_left = \ 402 | _get_steer_ang(math.atan(self._inv_wheelbase * 403 | (center_y - self._joint_dist_div_2))) 404 | self._theta_right = \ 405 | _get_steer_ang(math.atan(self._inv_wheelbase * 406 | (center_y + self._joint_dist_div_2))) 407 | 408 | return steer_ang_changed, center_y 409 | 410 | def _ctrl_axles(self, speed, accel_limit, jerk_limit, delta_t, 411 | steer_ang_changed, center_y): 412 | # Control the axle joints. 413 | 414 | # Compute veh_speed, the vehicle's desired speed. 415 | if accel_limit > 0.0: 416 | # Limit the vehicle's acceleration. 417 | 418 | if jerk_limit > 0.0: 419 | if self._last_accel_limit > 0.0: 420 | jerk = (accel_limit - self._last_accel_limit) / delta_t 421 | jerk = max(-jerk_limit, min(jerk, jerk_limit)) 422 | accel_limit_2 = self._last_accel_limit + jerk * delta_t 423 | else: 424 | accel_limit_2 = accel_limit 425 | else: 426 | accel_limit_2 = accel_limit 427 | self._last_accel_limit = accel_limit_2 428 | 429 | accel = (speed - self._last_speed) / delta_t 430 | accel = max(-accel_limit_2, min(accel, accel_limit_2)) 431 | veh_speed = self._last_speed + accel * delta_t 432 | else: 433 | self._last_accel_limit = accel_limit 434 | veh_speed = speed 435 | 436 | # Compute the desired angular velocities of the wheels. 437 | if veh_speed != self._last_speed or steer_ang_changed: 438 | self._last_speed = veh_speed 439 | left_dist = center_y - self._joint_dist_div_2 440 | right_dist = center_y + self._joint_dist_div_2 441 | 442 | # Front 443 | gain = (2 * pi) * veh_speed / abs(center_y) 444 | r = math.sqrt(left_dist ** 2 + self._wheelbase_sqr) 445 | self._left_front_ang_vel = gain * r * self._left_front_inv_circ 446 | r = math.sqrt(right_dist ** 2 + self._wheelbase_sqr) 447 | self._right_front_ang_vel = gain * r * self._right_front_inv_circ 448 | # Rear 449 | gain = (2 * pi) * veh_speed / center_y 450 | self._left_rear_ang_vel = \ 451 | gain * left_dist * self._left_rear_inv_circ 452 | self._right_rear_ang_vel = \ 453 | gain * right_dist * self._right_rear_inv_circ 454 | 455 | _DEF_WHEEL_DIA = 1.0 # Default wheel diameter. Unit: meter. 456 | _DEF_EQ_POS = 0.0 # Default equilibrium position. Unit: meter. 457 | _DEF_CMD_TIMEOUT = 0.5 # Default command timeout. Unit: second. 458 | _DEF_PUB_FREQ = 30.0 # Default publishing frequency. Unit: hertz. 459 | # end _AckermannCtrlr 460 | 461 | 462 | def _wait_for_ctrlr(list_ctrlrs, ctrlr_name): 463 | # Wait for the specified controller to be in the "running" state. 464 | # Commands can be lost if they are published before their controller is 465 | # running, even if a latched publisher is used. 466 | 467 | while True: 468 | response = list_ctrlrs() 469 | for ctrlr in response.controller: 470 | if ctrlr.name == ctrlr_name: 471 | if ctrlr.state == "running": 472 | return 473 | rospy.sleep(0.1) 474 | break 475 | 476 | 477 | def _create_axle_cmd_pub(list_ctrlrs, axle_ctrlr_name): 478 | # Create an axle command publisher. 479 | if not axle_ctrlr_name: 480 | return None 481 | return _create_cmd_pub(list_ctrlrs, axle_ctrlr_name) 482 | 483 | 484 | def _create_cmd_pub(list_ctrlrs, ctrlr_name): 485 | # Create a command publisher. 486 | _wait_for_ctrlr(list_ctrlrs, ctrlr_name) 487 | return rospy.Publisher(ctrlr_name + "/command", Float64, queue_size=10) 488 | 489 | 490 | def _get_steer_ang(phi): 491 | # Return the desired steering angle for a front wheel. 492 | if phi >= 0.0: 493 | return (pi / 2) - phi 494 | return (-pi / 2) - phi 495 | 496 | 497 | # main 498 | if __name__ == "__main__": 499 | ctrlr = _AckermannCtrlr() 500 | ctrlr.spin() 501 | --------------------------------------------------------------------------------