├── .gitignore ├── README.md ├── youbot_gazebo_control ├── CMakeLists.txt ├── config │ ├── arm_1_controller.yaml │ ├── arm_1_gripper_controller.yaml │ ├── arm_1_vel_controller.yaml │ ├── arm_2_controller.yaml │ ├── arm_2_gripper_controller.yaml │ ├── base_controller.yaml │ └── joint_state_controller.yaml ├── launch │ ├── arm_controller.launch │ ├── base_controller.launch │ ├── gripper_controller.launch │ ├── joint_state_controller.launch │ └── vel_arm_controller.launch ├── package.xml ├── src │ ├── arm_joint_vel_convertor.cpp │ └── steered_wheel_base_controller.cpp └── youbot_gazebo_control_plugins.xml ├── youbot_gazebo_robot ├── CMakeLists.txt ├── launch │ ├── vel_youbot_arm.launch │ ├── youbot.launch │ ├── youbot_arm_only.launch │ ├── youbot_base_only.launch │ ├── youbot_dual_arm.launch │ └── youbot_with_cam3d.launch └── package.xml ├── youbot_gazebo_worlds ├── CMakeLists.txt ├── launch │ ├── empty_world.launch │ ├── robocup_at_work_2012.launch │ └── tower_of_hanoi.launch ├── package.xml └── urdf │ ├── robocup_at_work_2012.urdf │ └── tower_of_hanoi.urdf └── youbot_simulation ├── CMakeLists.txt └── package.xml /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | .autotools 3 | .cproject 4 | .project 5 | .pydevproject 6 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | youbot_simulation 2 | ================= 3 | 4 | Packages to run the KUKA youBot in the Gazebo simulation with ROS 5 | -------------------------------------------------------------------------------- /youbot_gazebo_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(youbot_gazebo_control) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | angles 7 | control_toolbox 8 | controller_interface 9 | geometry_msgs 10 | brics_actuator 11 | hardware_interface 12 | kdl_parser 13 | nav_msgs 14 | pluginlib 15 | realtime_tools 16 | roscpp 17 | roslint 18 | tf 19 | urdf 20 | ) 21 | 22 | catkin_package( 23 | LIBRARIES 24 | steered_wheel_base_controller 25 | CATKIN_DEPENDS 26 | controller_interface 27 | hardware_interface 28 | ) 29 | 30 | include_directories( 31 | ${catkin_INCLUDE_DIRS} 32 | ) 33 | 34 | ### LIBS 35 | add_library(steered_wheel_base_controller 36 | src/steered_wheel_base_controller.cpp 37 | ) 38 | target_link_libraries(steered_wheel_base_controller 39 | ${catkin_LIBRARIES} 40 | ) 41 | 42 | add_executable(arm_joint_vel_convertor 43 | src/arm_joint_vel_convertor.cpp 44 | ) 45 | target_link_libraries(arm_joint_vel_convertor 46 | ${catkin_LIBRARIES} 47 | ) 48 | 49 | ### TESTS 50 | roslint_cpp() 51 | 52 | if(CATKIN_ENABLE_TESTING) 53 | find_package(roslaunch REQUIRED) 54 | 55 | roslaunch_add_file_check(launch) 56 | endif() 57 | 58 | 59 | ### INSTALLS 60 | install(TARGETS steered_wheel_base_controller 61 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 62 | ) 63 | 64 | install(DIRECTORY config/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config) 65 | install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) 66 | install(FILES youbot_gazebo_control_plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 67 | -------------------------------------------------------------------------------- /youbot_gazebo_control/config/arm_1_controller.yaml: -------------------------------------------------------------------------------- 1 | arm_controller: 2 | type: effort_controllers/JointTrajectoryController 3 | joints: 4 | - arm_joint_1 5 | - arm_joint_2 6 | - arm_joint_3 7 | - arm_joint_4 8 | - arm_joint_5 9 | gains: 10 | arm_joint_1: {p: 1000.0, i: 0.01, d: 0.1} 11 | arm_joint_2: {p: 1000.0, i: 0.01, d: 0.1} 12 | arm_joint_3: {p: 1000.0, i: 0.01, d: 0.1} 13 | arm_joint_4: {p: 1000.0, i: 0.01, d: 0.1} 14 | arm_joint_5: {p: 1000.0, i: 0.01, d: 0.1} 15 | constraints: 16 | goal_time: 0.6 17 | arm_joint_1: 18 | goal: 0.1 19 | arm_joint_2: 20 | goal: 0.1 21 | arm_joint_3: 22 | goal: 0.1 23 | arm_joint_4: 24 | goal: 0.1 25 | arm_joint_5: 26 | goal: 0.1 27 | -------------------------------------------------------------------------------- /youbot_gazebo_control/config/arm_1_gripper_controller.yaml: -------------------------------------------------------------------------------- 1 | gripper_controller: 2 | type: effort_controllers/JointTrajectoryController 3 | joints: 4 | - gripper_finger_joint_l 5 | - gripper_finger_joint_r 6 | gains: 7 | gripper_finger_joint_l: {p: 1000.0, d: 1.0} 8 | gripper_finger_joint_r: {p: 1000.0, d: 1.0} 9 | -------------------------------------------------------------------------------- /youbot_gazebo_control/config/arm_1_vel_controller.yaml: -------------------------------------------------------------------------------- 1 | joint_1_velocity: 2 | type: "effort_controllers/JointVelocityController" 3 | joint: arm_joint_1 4 | pid: {p: 2.0, i: 100.0, d: 0.0, i_clamp_min: -100.0, i_clamp_max: 100.0, antiwindup: true} 5 | 6 | joint_2_velocity: 7 | type: "effort_controllers/JointVelocityController" 8 | joint: arm_joint_2 9 | pid: {p: 1.0, i: 100.0, d: 0.0, i_clamp_min: -100.0, i_clamp_max: 100.0, antiwindup: true} 10 | 11 | joint_3_velocity: 12 | type: "effort_controllers/JointVelocityController" 13 | joint: arm_joint_3 14 | pid: {p: 1.0, i: 100.0, d: 0.0, i_clamp_min: -1000.0, i_clamp_max: 1000.0, antiwindup: true} 15 | 16 | joint_4_velocity: 17 | type: "effort_controllers/JointVelocityController" 18 | joint: arm_joint_4 19 | pid: {p: 1.0, i: 100.0, d: 0.0, i_clamp_min: -1000.0, i_clamp_max: 1000.0, antiwindup: true} 20 | 21 | joint_5_velocity: 22 | type: "effort_controllers/JointVelocityController" 23 | joint: arm_joint_5 24 | pid: {p: 5.0, i: 100.0, d: 0.0, i_clamp_min: -100.0, i_clamp_max: 100.0, antiwindup: true} 25 | -------------------------------------------------------------------------------- /youbot_gazebo_control/config/arm_2_controller.yaml: -------------------------------------------------------------------------------- 1 | arm_controller: 2 | type: effort_controllers/JointTrajectoryController 3 | joints: 4 | - arm_joint_1 5 | - arm_joint_2 6 | - arm_joint_3 7 | - arm_joint_4 8 | - arm_joint_5 9 | gains: 10 | arm_joint_1: {p: 1000.0, i: 0.01, d: 0.1} 11 | arm_joint_2: {p: 1000.0, i: 0.01, d: 0.1} 12 | arm_joint_3: {p: 1000.0, i: 0.01, d: 0.1} 13 | arm_joint_4: {p: 1000.0, i: 0.01, d: 0.1} 14 | arm_joint_5: {p: 1000.0, i: 0.01, d: 0.1} 15 | constraints: 16 | goal_time: 0.6 17 | arm_joint_1: 18 | goal: 0.1 19 | arm_joint_2: 20 | goal: 0.1 21 | arm_joint_3: 22 | goal: 0.1 23 | arm_joint_4: 24 | goal: 0.1 25 | arm_joint_5: 26 | goal: 0.1 27 | -------------------------------------------------------------------------------- /youbot_gazebo_control/config/arm_2_gripper_controller.yaml: -------------------------------------------------------------------------------- 1 | gripper_controller: 2 | type: effort_controllers/JointTrajectoryController 3 | joints: 4 | - gripper_finger_joint_l 5 | - gripper_finger_joint_r 6 | gains: 7 | gripper_finger_joint_l: {p: 1000.0, d: 1.0} 8 | gripper_finger_joint_r: {p: 1000.0, d: 1.0} 9 | -------------------------------------------------------------------------------- /youbot_gazebo_control/config/base_controller.yaml: -------------------------------------------------------------------------------- 1 | base_controller: 2 | pid_gains: 3 | wheel_joint_fl: {p: 100.0, i: 0.0, d: 1.0} 4 | caster_joint_fl: {p: 100.0, i: 0.0, d: 1.0} 5 | wheel_joint_fr: {p: 100.0, i: 0.0, d: 1.0} 6 | caster_joint_fr: {p: 100.0, i: 0.0, d: 1.0} 7 | wheel_joint_bl: {p: 100.0, i: 0.0, d: 1.0} 8 | caster_joint_bl: {p: 100.0, i: 0.0, d: 1.0} 9 | wheel_joint_br: {p: 100.0, i: 0.0, d: 1.0} 10 | caster_joint_br: {p: 100.0, i: 0.0, d: 1.0} 11 | 12 | type: steered_wheel_base_controller/SteeredWheelBaseController 13 | 14 | base_frame: base_footprint 15 | 16 | linear_speed_limit: 1.5 17 | linear_acceleration_limit: 2.5 18 | linear_deceleration_limit: 2.5 19 | 20 | yaw_speed_limit: 3.0 21 | yaw_acceleration_limit: 3.2 22 | yaw_deceleration_limit: 3.2 23 | 24 | cmd_vel_timeout: 0.0 # disabled, as it is on the real platform 25 | 26 | wheels: 27 | - steering_joint: caster_joint_fl 28 | axle_joint: wheel_joint_fl 29 | diameter: 0.095 30 | - steering_joint: caster_joint_fr 31 | axle_joint: wheel_joint_fr 32 | diameter: 0.095 33 | - steering_joint: caster_joint_bl 34 | axle_joint: wheel_joint_bl 35 | diameter: 0.095 36 | - steering_joint: caster_joint_br 37 | axle_joint: wheel_joint_br 38 | diameter: 0.095 39 | -------------------------------------------------------------------------------- /youbot_gazebo_control/config/joint_state_controller.yaml: -------------------------------------------------------------------------------- 1 | joint_state_controller: 2 | type: joint_state_controller/JointStateController 3 | publish_rate: 100 4 | -------------------------------------------------------------------------------- /youbot_gazebo_control/launch/arm_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /youbot_gazebo_control/launch/base_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /youbot_gazebo_control/launch/gripper_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /youbot_gazebo_control/launch/joint_state_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /youbot_gazebo_control/launch/vel_arm_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 18 | 19 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /youbot_gazebo_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | youbot_gazebo_control 4 | 0.1.0 5 | Controller 6 | 7 | Walter Nowak 8 | Sebastian Blumenthal 9 | 10 | GPLv3 11 | 12 | http://github.com/mas-group/youbot_simulation --> 13 | 14 | Frederik Hegger 15 | 16 | catkin 17 | 18 | angles 19 | control_toolbox 20 | controller_interface 21 | geometry_msgs 22 | brics_actuator 23 | hardware_interface 24 | kdl_parser 25 | nav_msgs 26 | orocos_kdl 27 | pluginlib 28 | realtime_tools 29 | roscpp 30 | roslint 31 | tf 32 | urdf 33 | 34 | controller_interface 35 | controller_manager 36 | hardware_interface 37 | joint_state_controller 38 | joint_trajectory_controller 39 | brics_actuator 40 | 41 | roslaunch 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /youbot_gazebo_control/src/arm_joint_vel_convertor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This node converts commands from brics_actuator::JointVelocity type to 3 | * individual joint's velocity command for simulated youbot arm. 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | std::vector joint_pubs; 11 | ros::Subscriber joint_vel_sub; 12 | ros::Timer step_timer; 13 | size_t num_of_joints = 5; 14 | std::vector joint_vel; 15 | std::vector expected_joint_uri = {"arm_joint_1", "arm_joint_2", "arm_joint_3", 16 | "arm_joint_4", "arm_joint_5"}; 17 | std::string expected_unit = "s^-1 rad"; 18 | 19 | void jointVelCb(const brics_actuator::JointVelocities::ConstPtr& msg) 20 | { 21 | if ( msg->velocities.size() != num_of_joints ) 22 | { 23 | ROS_ERROR_STREAM("Number of joints do not match. Expecting " 24 | << num_of_joints << ", got " << msg->velocities.size() << "."); 25 | return; 26 | } 27 | 28 | for ( size_t i = 0; i < num_of_joints; i++ ) 29 | { 30 | if ( msg->velocities[i].joint_uri != expected_joint_uri[i] ) 31 | { 32 | ROS_ERROR_STREAM("joint_uri does not match. Expecting " 33 | << expected_joint_uri[i] << ", got " 34 | << msg->velocities[i].joint_uri << "."); 35 | for ( size_t j = 0; j < num_of_joints; j++ ) 36 | { 37 | joint_vel[j] = 0.0f; 38 | } 39 | return; 40 | } 41 | 42 | if ( msg->velocities[i].unit != expected_unit ) 43 | { 44 | ROS_ERROR_STREAM("unit does not match. Expecting " 45 | << expected_unit << ", got " 46 | << msg->velocities[i].unit << "."); 47 | for ( size_t j = 0; j < num_of_joints; j++ ) 48 | { 49 | joint_vel[j] = 0.0f; 50 | } 51 | return; 52 | } 53 | 54 | joint_vel[i] = msg->velocities[i].value; 55 | } 56 | }; 57 | 58 | void step() 59 | { 60 | for ( size_t i = 0; i < num_of_joints; i++ ) 61 | { 62 | std_msgs::Float64 command_msg; 63 | command_msg.data = joint_vel[i]; 64 | joint_pubs[i].publish(command_msg); 65 | } 66 | }; 67 | 68 | void init() 69 | { 70 | ros::NodeHandle nh("~"); 71 | std::string arm_name; 72 | nh.param("arm_name", arm_name, "arm_1"); 73 | std::string arm_joint_vel_topic = "/" + arm_name + "/arm_controller/velocity_command"; 74 | 75 | joint_vel = std::vector(5, 0.0f); 76 | 77 | for ( size_t i = 0; i < num_of_joints; i++ ) 78 | { 79 | std::stringstream joint_command_topic; 80 | joint_command_topic << "/" << arm_name << "/joint_" << i+1 << "_velocity/command"; 81 | joint_pubs.push_back(nh.advertise(joint_command_topic.str(), 1)); 82 | } 83 | 84 | /* subscribers */ 85 | joint_vel_sub = nh.subscribe(arm_joint_vel_topic, 5, jointVelCb); 86 | 87 | }; 88 | 89 | int main(int argc, char **argv) 90 | { 91 | ros::init(argc, argv, "arm_joint_vel_convertor"); 92 | init(); 93 | ros::NodeHandle nh("~"); 94 | 95 | /* ros params */ 96 | float loop_rate; 97 | nh.param("loop_rate", loop_rate, 10.0f); 98 | ros::Rate rate(loop_rate); 99 | 100 | while ( ros::ok() ) 101 | { 102 | step(); 103 | ros::spinOnce(); 104 | rate.sleep(); 105 | } 106 | 107 | return 0; 108 | } 109 | -------------------------------------------------------------------------------- /youbot_gazebo_control/src/steered_wheel_base_controller.cpp: -------------------------------------------------------------------------------- 1 | /// \file steered_wheel_base_controller.cpp 2 | /// 3 | /// \brief Steered-wheel base controller 4 | /// 5 | /// Copyright (c) 2013-2014 Wunderkammer Laboratory 6 | /// 7 | /// This file contains the source code for SteeredWheelBaseController, 8 | /// a base controller for mobile robots. It works with bases that have two or 9 | /// more independently-steerable driven wheels and zero or more omnidirectional 10 | /// passive wheels (e.g. swivel casters). 11 | /// 12 | /// Subscribed Topics: 13 | /// cmd_vel (geometry_msgs/Twist) 14 | /// Velocity command, defined in the frame specified by the base_link 15 | /// parameter. The linear.x and linear.y fields specify the base's 16 | /// desired linear velocity, measured in meters per second. 17 | /// The angular.z field specifies the base's desired angular velocity, 18 | /// measured in radians per second. 19 | /// 20 | /// Published Topics: 21 | /// odom (nav_msgs/Odometry) 22 | /// Odometry. 23 | /// 24 | /// Parameters: 25 | /// ~robot_description_name (string, default: robot_description) 26 | /// Name of a parameter on the Parameter Server. The named parameter's 27 | /// value is URDF data that describes the robot. 28 | /// ~base_link (string, default: base_link) 29 | /// Link that specifies the frame in which cmd_vel is defined. 30 | /// The link specified by base_link must exist in the robot's URDF 31 | /// data. 32 | /// ~cmd_vel_timeout (float, default: 0.5) 33 | /// If cmd_vel_timeout is greater than zero and this controller does 34 | /// not receive a velocity command for more than cmd_vel_timeout 35 | /// seconds, wheel motion is paused until a command is received. 36 | /// If cmd_vel_timeout is less than or equal to zero, the command 37 | /// timeout is disabled. 38 | /// 39 | /// ~linear_speed_limit (float, default: 1.0) 40 | /// Linear speed limit. If linear_speed_limit is less than zero, the 41 | /// linear speed limit is disabled. Unit: m/s. 42 | /// ~linear_acceleration_limit (float, default: 1.0) 43 | /// Linear acceleration limit. If linear_acceleration_limit is less 44 | /// than zero, the linear acceleration limit is disabled. Unit: m/s**2. 45 | /// ~linear_deceleration_limit (float, default: -1.0) 46 | /// Linear deceleration limit. If linear_deceleration_limit is less 47 | /// than or equal to zero, the linear deceleration limit is disabled. 48 | /// Unit: m/s**2. 49 | /// 50 | /// ~yaw_speed_limit (float, default: 1.0) 51 | /// Yaw speed limit. If yaw_speed_limit is less than zero, the yaw 52 | /// speed limit is disabled. Unit: rad/s. 53 | /// ~yaw_acceleration_limit (float, default: 1.0) 54 | /// Yaw acceleration limit. If yaw_acceleration_limit is less than 55 | /// zero, the yaw acceleration limit is disabled. Unit: rad/s**2. 56 | /// ~yaw_deceleration_limit (float, default: -1.0) 57 | /// Yaw deceleration limit. If yaw_deceleration_limit is less than or 58 | /// equal to zero, the yaw deceleration limit is disabled. 59 | /// Unit: rad/s**2. 60 | /// 61 | /// ~full_axle_speed_angle (float, default: 0.7854) 62 | /// If the difference between a wheel's desired and measured steering 63 | /// angles is less than or equal to full_axle_speed_angle, the wheel's 64 | /// axle will rotate at the speed determined by the current velocity 65 | /// command, subject to the speed, acceleration, and deceleration 66 | /// limits. full_axle_speed_angle must be less than 67 | /// zero_axle_speed_angle. Range: [0, pi]. Unit: radian. 68 | /// ~zero_axle_speed_angle (float, default: 1.5708) 69 | /// If the difference between a wheel's desired and measured steering 70 | /// angles is greater than or equal to zero_axle_speed_angle, the 71 | /// wheel's axle will stop rotating, subject to the deceleration 72 | /// limits. zero_axle_speed_angle must be greater than 73 | /// full_axle_speed_angle. Range: [0, pi]. Unit: radian. 74 | /// 75 | /// ~wheels (sequence of mappings, default: empty) 76 | /// Two or more steered wheels. 77 | /// 78 | /// Key-Value Pairs: 79 | /// 80 | /// steering_joint (string) 81 | /// Steering joint. 82 | /// axle_joint (string) 83 | /// Axle joint. 84 | /// diameter (float) 85 | /// Wheel diameter. It must be greater than zero. Unit: meter. 86 | /// ~wheel_diameter_scale (float, default: 1.0) 87 | /// Scale applied to each wheel's diameter. It is used to correct for 88 | /// tire deformation. wheel_diameter_scale must be greater than zero. 89 | /// ~pid_gains/ (mapping, default: empty) 90 | /// PID controller gains for the specified joint. Needed only for 91 | /// effort-controlled joints and velocity-controlled steering joints. 92 | /// 93 | /// ~odometry_publishing_frequency (float, default: 30.0) 94 | /// Odometry publishing frequency. If it is less than or equal to zero, 95 | /// odometry computation is disabled. Unit: hertz. 96 | /// ~odometry_frame (string, default: odom) 97 | /// Odometry frame. 98 | /// ~base_frame (string, default: base_link) 99 | /// Base frame in the -to- transform 100 | /// provided by this controller. base_frame allows the controller to 101 | /// publish transforms from odometry_frame to a frame that is not a 102 | /// link in the robot's URDF data. For example, base_frame can be set 103 | /// to "base_footprint". 104 | /// ~initial_x (float, default: 0.0) 105 | /// X coordinate of the base frame's initial position in the odometry 106 | /// frame. Unit: meter. 107 | /// ~initial_y (float, default: 0.0) 108 | /// Y coordinate of the base frame's initial position in the odometry 109 | /// frame. Unit: meter. 110 | /// ~initial_yaw (float, default: 0.0) 111 | /// Initial orientation of the base frame in the odometry frame. 112 | /// Range: [-pi, pi]. Unit: radian. 113 | /// 114 | /// Provided tf Transforms: 115 | /// to 116 | /// Specifies the base frame's pose in the odometry frame. 117 | // 118 | // 119 | // Licensed under the Apache License, Version 2.0 (the "License"); 120 | // you may not use this file except in compliance with the License. 121 | // You may obtain a copy of the License at 122 | // 123 | // http://www.apache.org/licenses/LICENSE-2.0 124 | // 125 | // Unless required by applicable law or agreed to in writing, software 126 | // distributed under the License is distributed on an "AS IS" BASIS, 127 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 128 | // See the License for the specific language governing permissions and 129 | // limitations under the License. 130 | // 131 | // Modified by: Frederik Hegger (20.08.2015) 132 | 133 | #include 134 | #include 135 | #include 136 | #include 137 | #include 138 | #include 139 | 140 | #include 141 | 142 | #include 143 | #include 144 | #include 145 | 146 | #include 147 | #include 148 | 149 | #include 150 | #include 151 | #include 152 | #include 153 | #include 154 | #include 155 | #include 156 | 157 | #include 158 | #include 159 | 160 | #include 161 | #include 162 | #include 163 | 164 | #include 165 | #include 166 | 167 | #include 168 | 169 | using std::runtime_error; 170 | using std::set; 171 | using std::string; 172 | 173 | using boost::math::sign; 174 | using boost::shared_ptr; 175 | 176 | using controller_interface::ControllerBase; 177 | 178 | using Eigen::Affine2d; 179 | using Eigen::Matrix2d; 180 | using Eigen::Vector2d; 181 | 182 | using geometry_msgs::TwistConstPtr; 183 | 184 | using hardware_interface::JointHandle; 185 | using hardware_interface::RobotHW; 186 | 187 | using hardware_interface::EffortJointInterface; 188 | using hardware_interface::PositionJointInterface; 189 | using hardware_interface::VelocityJointInterface; 190 | 191 | using realtime_tools::RealtimeBuffer; 192 | using realtime_tools::RealtimePublisher; 193 | 194 | using ros::Duration; 195 | using ros::NodeHandle; 196 | using ros::Time; 197 | 198 | using XmlRpc::XmlRpcValue; 199 | 200 | namespace { 201 | 202 | void addClaimedResources(hardware_interface::HardwareInterface *const hw_iface, std::string hw_iface_name, controller_interface::ControllerBase::ClaimedResources& claimed_resources){ 203 | if (hw_iface == NULL) return; 204 | 205 | hardware_interface::InterfaceResources iface_res(hw_iface_name, hw_iface->getClaims()); 206 | if (!iface_res.resources.empty()) 207 | claimed_resources.push_back(iface_res); 208 | hw_iface->clearClaims(); 209 | } 210 | 211 | double clamp(const double val, const double min_val, const double max_val) 212 | { 213 | return std::min(std::max(val, min_val), max_val); 214 | } 215 | 216 | double hermite(const double t) 217 | { 218 | if (t <= 0) 219 | return 0; 220 | if (t >= 1) 221 | return 1; 222 | return (-2 * t + 3) * t * t; // -2t**3 + 3t**2 223 | } 224 | 225 | class Joint 226 | { 227 | public: 228 | virtual ~Joint() {} 229 | virtual void init() = 0; 230 | virtual void stop() = 0; 231 | 232 | bool isValidPos(const double pos); 233 | double getPos() const 234 | { 235 | return handle_.getPosition(); 236 | } 237 | virtual void setPos(const double pos, const Duration& period) {} 238 | 239 | virtual void setVel(const double vel, const Duration& period) = 0; 240 | 241 | protected: 242 | Joint(const JointHandle& handle, 243 | const urdf::JointConstSharedPtr urdf_joint); 244 | 245 | JointHandle handle_; 246 | const double lower_limit_, upper_limit_; // Unit: radian 247 | bool check_joint_limit_; 248 | }; 249 | 250 | // Position-controlled joint 251 | class PosJoint : public Joint 252 | { 253 | public: 254 | PosJoint(const JointHandle& handle, 255 | const urdf::JointConstSharedPtr urdf_joint) : 256 | Joint(handle, urdf_joint) {} 257 | 258 | virtual void init(); 259 | virtual void stop(); 260 | virtual void setPos(const double pos, const Duration& period); 261 | virtual void setVel(const double vel, const Duration& period); 262 | 263 | private: 264 | double pos_; 265 | }; 266 | 267 | // Velocity-controlled joint. VelJoint is used for axles only. 268 | class VelJoint : public Joint 269 | { 270 | public: 271 | VelJoint(const JointHandle& handle, 272 | const urdf::JointConstSharedPtr urdf_joint) : 273 | Joint(handle, urdf_joint) {} 274 | 275 | virtual void init() 276 | { 277 | stop(); 278 | } 279 | virtual void stop(); 280 | virtual void setVel(const double vel, const Duration& period); 281 | }; 282 | 283 | // An object of class PIDJoint is a joint controlled by a PID controller. 284 | class PIDJoint : public Joint 285 | { 286 | public: 287 | PIDJoint(const JointHandle& handle, 288 | const urdf::JointConstSharedPtr urdf_joint, 289 | const NodeHandle& ctrlr_nh); 290 | 291 | virtual void init(); 292 | virtual void stop(); 293 | virtual void setPos(const double pos, const Duration& period); 294 | virtual void setVel(const double vel, const Duration& period); 295 | 296 | private: 297 | const int type_; // URDF joint type 298 | control_toolbox::Pid pid_ctrlr_; 299 | }; 300 | 301 | // An object of class Wheel is a steered wheel. 302 | class Wheel 303 | { 304 | public: 305 | Wheel(const KDL::Tree& tree, 306 | const string& base_link, const string& steer_link, 307 | const shared_ptr steer_joint, 308 | const shared_ptr axle_joint, 309 | const double circ); 310 | 311 | const Vector2d& pos() const 312 | { 313 | return pos_; 314 | } 315 | Vector2d getDeltaPos(); 316 | 317 | void initJoints(); 318 | void stop() const; 319 | double ctrlSteering(const Duration& period, const double hermite_scale, 320 | const double hermite_offset); 321 | double ctrlSteering(const double theta_desired, const Duration& period, 322 | const double hermite_scale, const double hermite_offset); 323 | void ctrlAxle(const double lin_speed, const Duration& period) const; 324 | 325 | private: 326 | void initPos(const KDL::Tree& tree, const string& base_link); 327 | 328 | string steer_link_; // Steering link 329 | Vector2d pos_; // Wheel's position in the base link's frame 330 | 331 | shared_ptr steer_joint_; // Steering joint 332 | shared_ptr axle_joint_; 333 | double theta_steer_; // Steering joint position 334 | double last_theta_steer_desired_; // Last desired steering joint position 335 | double last_theta_axle_; // Last axle joint position 336 | 337 | double radius_; // Unit: meter. 338 | double inv_radius_; // Inverse of radius_ 339 | double axle_vel_gain_; // Axle velocity gain 340 | }; 341 | 342 | Joint::Joint(const JointHandle& handle, 343 | const urdf::JointConstSharedPtr urdf_joint) : 344 | handle_(handle), lower_limit_(urdf_joint->limits->lower), 345 | upper_limit_(urdf_joint->limits->upper), check_joint_limit_(true) 346 | { 347 | // Do nothing. 348 | } 349 | 350 | bool Joint::isValidPos(const double pos) 351 | { 352 | if (!check_joint_limit_) 353 | return true; 354 | 355 | return pos >= lower_limit_ && pos <= upper_limit_; 356 | } 357 | 358 | // Initialize this joint. 359 | void PosJoint::init() 360 | { 361 | pos_ = getPos(); 362 | stop(); 363 | } 364 | 365 | // Stop this joint's motion. 366 | void PosJoint::stop() 367 | { 368 | handle_.setCommand(getPos()); 369 | } 370 | 371 | // Specify this joint's position. 372 | void PosJoint::setPos(const double pos, const Duration& /* period */) 373 | { 374 | pos_ = pos; 375 | handle_.setCommand(pos_); 376 | } 377 | 378 | // Specify this joint's velocity. 379 | void PosJoint::setVel(const double vel, const Duration& period) 380 | { 381 | pos_ += vel * period.toSec(); 382 | handle_.setCommand(pos_); 383 | } 384 | 385 | // Stop this joint's motion. 386 | void VelJoint::stop() 387 | { 388 | handle_.setCommand(0); 389 | } 390 | 391 | // Specify this joint's velocity. 392 | void VelJoint::setVel(const double vel, const Duration& /* period */) 393 | { 394 | handle_.setCommand(vel); 395 | } 396 | 397 | PIDJoint::PIDJoint(const JointHandle& handle, 398 | const urdf::JointConstSharedPtr urdf_joint, 399 | const NodeHandle& ctrlr_nh) : 400 | Joint(handle, urdf_joint), type_(urdf_joint->type) 401 | { 402 | const NodeHandle nh(ctrlr_nh, "pid_gains/" + handle.getName()); 403 | if (!pid_ctrlr_.init(nh)) 404 | { 405 | throw runtime_error("No PID gain values for \"" + handle.getName() + 406 | "\" were found."); 407 | } 408 | } 409 | 410 | // Initialize this joint. 411 | void PIDJoint::init() 412 | { 413 | pid_ctrlr_.reset(); 414 | stop(); 415 | } 416 | 417 | // Stop this joint's motion. 418 | void PIDJoint::stop() 419 | { 420 | // The command passed to setCommand() might be an effort value or a 421 | // velocity. In either case, the correct command to pass here is zero. 422 | handle_.setCommand(0); 423 | } 424 | 425 | // Specify this joint's position. 426 | void PIDJoint::setPos(const double pos, const Duration& period) 427 | { 428 | const double curr_pos = getPos(); 429 | 430 | double error; 431 | switch (type_) 432 | { 433 | case urdf::Joint::REVOLUTE: 434 | angles::shortest_angular_distance_with_limits(curr_pos, pos, 435 | lower_limit_, upper_limit_, 436 | error); 437 | check_joint_limit_ = true; 438 | break; 439 | case urdf::Joint::CONTINUOUS: 440 | error = angles::shortest_angular_distance(curr_pos, pos); 441 | check_joint_limit_ = false; 442 | break; 443 | default: 444 | error = pos - curr_pos; 445 | break; 446 | } 447 | 448 | handle_.setCommand(pid_ctrlr_.computeCommand(error, period)); 449 | } 450 | 451 | // Specify this joint's velocity. 452 | void PIDJoint::setVel(const double vel, const Duration& period) 453 | { 454 | const double error = vel - handle_.getVelocity(); 455 | handle_.setCommand(pid_ctrlr_.computeCommand(error, period)); 456 | } 457 | 458 | Wheel::Wheel(const KDL::Tree& tree, 459 | const string& base_link, const string& steer_link, 460 | const shared_ptr steer_joint, 461 | const shared_ptr axle_joint, 462 | const double circ) 463 | { 464 | steer_link_ = steer_link; 465 | initPos(tree, base_link); 466 | 467 | steer_joint_ = steer_joint; 468 | axle_joint_ = axle_joint; 469 | theta_steer_ = steer_joint_->getPos(); 470 | last_theta_steer_desired_ = theta_steer_; 471 | last_theta_axle_ = axle_joint_->getPos(); 472 | 473 | radius_ = circ / (2 * M_PI); 474 | inv_radius_ = 1 / radius_; 475 | axle_vel_gain_ = 0; 476 | } 477 | 478 | // Return the difference between this wheel's current position and its 479 | // position when getDeltaPos() was last called. The returned vector is defined 480 | // in the base link's frame. 481 | Vector2d Wheel::getDeltaPos() 482 | { 483 | const double theta_axle = axle_joint_->getPos(); 484 | const double delta_theta_axle = theta_axle - last_theta_axle_; 485 | last_theta_axle_ = theta_axle; 486 | const double vec_mag = delta_theta_axle * radius_; 487 | return Vector2d(cos(theta_steer_), sin(theta_steer_)) * vec_mag; 488 | } 489 | 490 | // Initialize this wheel's steering and axle joints. 491 | void Wheel::initJoints() 492 | { 493 | steer_joint_->init(); 494 | axle_joint_->init(); 495 | } 496 | 497 | // Stop this wheel's motion. 498 | void Wheel::stop() const 499 | { 500 | steer_joint_->stop(); 501 | axle_joint_->stop(); 502 | } 503 | 504 | // Maintain the position of this wheel's steering joint. Return a linear speed 505 | // gain value based on the difference between the desired steering angle and 506 | // the actual steering angle. 507 | double Wheel::ctrlSteering(const Duration& period, const double hermite_scale, 508 | const double hermite_offset) 509 | { 510 | return ctrlSteering(last_theta_steer_desired_, period, hermite_scale, 511 | hermite_offset); 512 | } 513 | 514 | // Control this wheel's steering joint. theta_desired range: [-pi, pi]. 515 | // Return a linear speed gain value based on the difference between the 516 | // desired steering angle and the actual steering angle. 517 | double Wheel::ctrlSteering(const double theta_desired, const Duration& period, 518 | const double hermite_scale, 519 | const double hermite_offset) 520 | { 521 | last_theta_steer_desired_ = theta_desired; 522 | 523 | // Find the minimum rotation that will align the wheel with theta_desired. 524 | theta_steer_ = steer_joint_->getPos(); 525 | const double theta_diff = fabs(theta_desired - theta_steer_); 526 | double theta; 527 | if (theta_diff > M_PI_2) 528 | { 529 | theta = theta_desired - copysign(M_PI, theta_desired); 530 | axle_vel_gain_ = -1; 531 | } 532 | else 533 | { 534 | theta = theta_desired; 535 | axle_vel_gain_ = 1; 536 | } 537 | 538 | // Keep theta within its valid range. 539 | if (!steer_joint_->isValidPos(theta)) 540 | { 541 | theta -= copysign(M_PI, theta); 542 | axle_vel_gain_ = -axle_vel_gain_; 543 | } 544 | 545 | steer_joint_->setPos(theta, period); 546 | return 1 - hermite(hermite_scale * (fabs(theta - theta_steer_) - 547 | hermite_offset)); 548 | } 549 | 550 | // Control this wheel's axle joint. 551 | void Wheel::ctrlAxle(const double lin_speed, const Duration& period) const 552 | { 553 | const double ang_vel = axle_vel_gain_ * inv_radius_ * lin_speed; 554 | axle_joint_->setVel(ang_vel, period); 555 | } 556 | 557 | // Initialize pos_. 558 | void Wheel::initPos(const KDL::Tree& tree, const string& base_link) 559 | { 560 | KDL::Chain chain; 561 | if (!tree.getChain(base_link, steer_link_, chain)) 562 | { 563 | throw runtime_error("No kinematic chain was found from \"" + base_link + 564 | "\" to \"" + steer_link_ + "\"."); 565 | } 566 | 567 | const unsigned int num_joints = chain.getNrOfJoints(); 568 | KDL::JntArray joint_positions(num_joints); 569 | for (unsigned int i = 0; i < num_joints; i++) 570 | joint_positions(i) = 0; 571 | 572 | KDL::ChainFkSolverPos_recursive solver(chain); 573 | KDL::Frame frame; 574 | if (solver.JntToCart(joint_positions, frame) < 0) 575 | { 576 | throw runtime_error("The position of steering link \"" + steer_link_ + 577 | "\" in base link frame \"" + base_link + 578 | "\" was not found."); 579 | } 580 | pos_ = Vector2d(frame.p.x(), frame.p.y()); 581 | } 582 | 583 | // Create an object of class Joint that corresponds to the URDF joint specified 584 | // by joint_name. 585 | shared_ptr getJoint(const string& joint_name, const bool is_steer_joint, 586 | const NodeHandle& ctrlr_nh, 587 | const urdf::Model& urdf_model, 588 | EffortJointInterface *const eff_joint_iface, 589 | PositionJointInterface *const pos_joint_iface, 590 | VelocityJointInterface *const vel_joint_iface) 591 | { 592 | if (eff_joint_iface != NULL) 593 | { 594 | JointHandle handle; 595 | bool handle_found; 596 | try 597 | { 598 | handle = eff_joint_iface->getHandle(joint_name); 599 | handle_found = true; 600 | } 601 | catch (...) 602 | { 603 | handle_found = false; 604 | } 605 | 606 | if (handle_found) 607 | { 608 | const urdf::JointConstSharedPtr urdf_joint = urdf_model.getJoint(joint_name); 609 | if (urdf_joint == NULL) 610 | { 611 | throw runtime_error("\"" + joint_name + 612 | "\" was not found in the URDF data."); 613 | } 614 | 615 | shared_ptr joint(new PIDJoint(handle, urdf_joint, ctrlr_nh)); 616 | return joint; 617 | } 618 | } 619 | 620 | if (pos_joint_iface != NULL) 621 | { 622 | JointHandle handle; 623 | bool handle_found; 624 | try 625 | { 626 | handle = pos_joint_iface->getHandle(joint_name); 627 | handle_found = true; 628 | } 629 | catch (...) 630 | { 631 | handle_found = false; 632 | } 633 | 634 | if (handle_found) 635 | { 636 | const urdf::JointConstSharedPtr urdf_joint = urdf_model.getJoint(joint_name); 637 | if (urdf_joint == NULL) 638 | { 639 | throw runtime_error("\"" + joint_name + 640 | "\" was not found in the URDF data."); 641 | } 642 | 643 | shared_ptr joint(new PosJoint(handle, urdf_joint)); 644 | return joint; 645 | } 646 | } 647 | 648 | if (vel_joint_iface != NULL) 649 | { 650 | JointHandle handle; 651 | bool handle_found; 652 | try 653 | { 654 | handle = vel_joint_iface->getHandle(joint_name); 655 | handle_found = true; 656 | } 657 | catch (...) 658 | { 659 | handle_found = false; 660 | } 661 | 662 | if (handle_found) 663 | { 664 | const urdf::JointConstSharedPtr urdf_joint = urdf_model.getJoint(joint_name); 665 | if (urdf_joint == NULL) 666 | { 667 | throw runtime_error("\"" + joint_name + 668 | "\" was not found in the URDF data."); 669 | } 670 | 671 | if (is_steer_joint) 672 | { 673 | shared_ptr joint(new PIDJoint(handle, urdf_joint, ctrlr_nh)); 674 | return joint; 675 | } 676 | shared_ptr joint(new VelJoint(handle, urdf_joint)); 677 | return joint; 678 | } 679 | } 680 | 681 | throw runtime_error("No handle for \"" + joint_name + "\" was found."); 682 | } 683 | 684 | } // namespace 685 | 686 | 687 | namespace steered_wheel_base_controller{ 688 | 689 | // Steered-wheel base controller 690 | class SteeredWheelBaseController : public controller_interface::ControllerBase 691 | { 692 | public: 693 | SteeredWheelBaseController(); 694 | 695 | // These are not real-time safe. 696 | virtual bool initRequest(RobotHW *const robot_hw, 697 | NodeHandle& root_nh, NodeHandle& ctrlr_nh, 698 | ClaimedResources& claimed_resources); 699 | virtual string getHardwareInterfaceType() const; 700 | 701 | // These are real-time safe. 702 | virtual void starting(const Time& time); 703 | virtual void update(const Time& time, const Duration& period); 704 | virtual void stopping(const Time& time); 705 | 706 | private: 707 | struct VelCmd // Velocity command 708 | { 709 | double x_vel; // X velocity component. Unit: m/s. 710 | double y_vel; // Y velocity component. Unit: m/s. 711 | double yaw_vel; // Yaw velocity. Unit: rad/s. 712 | 713 | // last_vel_cmd_time is the time at which the most recent velocity command 714 | // was received. 715 | Time last_vel_cmd_time; 716 | }; 717 | 718 | static const string DEF_ROBOT_DESC_NAME; 719 | static const string DEF_BASE_LINK; 720 | static const double DEF_CMD_VEL_TIMEOUT; 721 | 722 | static const double DEF_LIN_SPEED_LIMIT; 723 | static const double DEF_LIN_ACCEL_LIMIT; 724 | static const double DEF_LIN_DECEL_LIMIT; 725 | 726 | static const double DEF_YAW_SPEED_LIMIT; 727 | static const double DEF_YAW_ACCEL_LIMIT; 728 | static const double DEF_YAW_DECEL_LIMIT; 729 | 730 | static const double DEF_FULL_AXLE_SPEED_ANG; 731 | static const double DEF_ZERO_AXLE_SPEED_ANG; 732 | 733 | static const double DEF_WHEEL_DIA_SCALE; 734 | 735 | static const double DEF_ODOM_PUB_FREQ; 736 | static const string DEF_ODOM_FRAME; 737 | static const string DEF_BASE_FRAME; 738 | static const double DEF_INIT_X; 739 | static const double DEF_INIT_Y; 740 | static const double DEF_INIT_YAW; 741 | 742 | static const Vector2d X_DIR; 743 | 744 | void init(EffortJointInterface *const eff_joint_iface, 745 | PositionJointInterface *const pos_joint_iface, 746 | VelocityJointInterface *const vel_joint_iface, 747 | NodeHandle& ctrlr_nh); 748 | void velCmdCB(const TwistConstPtr& vel_cmd); 749 | 750 | Vector2d enforceLinLimits(const Vector2d& desired_vel, 751 | const double delta_t, const double inv_delta_t); 752 | double enforceYawLimits(const double desired_vel, 753 | const double delta_t, const double inv_delta_t); 754 | void ctrlWheels(const Vector2d& lin_vel, const double yaw_vel, 755 | const Duration& period); 756 | void compOdometry(const Time& time, const double inv_delta_t); 757 | 758 | std::vector wheels_; 759 | 760 | // Linear motion limits 761 | bool has_lin_speed_limit_; 762 | double lin_speed_limit_; 763 | bool has_lin_accel_limit_; 764 | double lin_accel_limit_; 765 | bool has_lin_decel_limit_; 766 | double lin_decel_limit_; 767 | 768 | // Yaw limits 769 | bool has_yaw_speed_limit_; 770 | double yaw_speed_limit_; 771 | bool has_yaw_accel_limit_; 772 | double yaw_accel_limit_; 773 | bool has_yaw_decel_limit_; 774 | double yaw_decel_limit_; 775 | 776 | double hermite_scale_, hermite_offset_; 777 | 778 | Vector2d last_lin_vel_; // Last linear velocity. Unit: m/s. 779 | double last_yaw_vel_; // Last yaw velocity. Unit: rad/s. 780 | 781 | // Velocity command member variables 782 | VelCmd vel_cmd_; 783 | RealtimeBuffer vel_cmd_buf_; 784 | bool vel_cmd_timeout_enabled_; 785 | Duration vel_cmd_timeout_; 786 | ros::Subscriber vel_cmd_sub_; 787 | 788 | // Odometry 789 | bool comp_odom_; // Compute odometry 790 | Duration odom_pub_period_; // Odometry publishing period 791 | Affine2d init_odom_to_base_; // Initial odometry to base frame transform 792 | Affine2d odom_to_base_; // Odometry to base frame transform 793 | Affine2d odom_affine_; 794 | double last_odom_x_, last_odom_y_, last_odom_yaw_; 795 | // wheel_pos_ contains the positions of the wheel's steering axles. 796 | // The positions are relative to the centroid of the steering axle positions 797 | // in the base link's frame. neg_wheel_centroid is the negative version of 798 | // that centroid. 799 | Eigen::Matrix2Xd wheel_pos_; 800 | Vector2d neg_wheel_centroid_; 801 | Eigen::MatrixX2d new_wheel_pos_; 802 | RealtimePublisher odom_pub_; 803 | RealtimePublisher odom_tf_pub_; 804 | Time last_odom_pub_time_, last_odom_tf_pub_time_; 805 | }; 806 | 807 | const string SteeredWheelBaseController::DEF_ROBOT_DESC_NAME = "robot_description"; // NOLINT(runtime/string) 808 | const string SteeredWheelBaseController::DEF_BASE_LINK = "base_link"; // NOLINT(runtime/string) 809 | const double SteeredWheelBaseController::DEF_CMD_VEL_TIMEOUT = 0.5; 810 | 811 | const double SteeredWheelBaseController::DEF_LIN_SPEED_LIMIT = 1; 812 | const double SteeredWheelBaseController::DEF_LIN_ACCEL_LIMIT = 1; 813 | const double SteeredWheelBaseController::DEF_LIN_DECEL_LIMIT = -1; 814 | 815 | const double SteeredWheelBaseController::DEF_YAW_SPEED_LIMIT = 1; 816 | const double SteeredWheelBaseController::DEF_YAW_ACCEL_LIMIT = 1; 817 | const double SteeredWheelBaseController::DEF_YAW_DECEL_LIMIT = -1; 818 | 819 | const double SteeredWheelBaseController::DEF_FULL_AXLE_SPEED_ANG = 0.7854; 820 | const double SteeredWheelBaseController::DEF_ZERO_AXLE_SPEED_ANG = 1.5708; 821 | 822 | const double SteeredWheelBaseController::DEF_WHEEL_DIA_SCALE = 1; 823 | 824 | const double SteeredWheelBaseController::DEF_ODOM_PUB_FREQ = 30; 825 | const string SteeredWheelBaseController::DEF_ODOM_FRAME = "odom"; // NOLINT(runtime/string) 826 | const string SteeredWheelBaseController::DEF_BASE_FRAME = "base_link"; // NOLINT(runtime/string) 827 | const double SteeredWheelBaseController::DEF_INIT_X = 0; 828 | const double SteeredWheelBaseController::DEF_INIT_Y = 0; 829 | const double SteeredWheelBaseController::DEF_INIT_YAW = 0; 830 | 831 | // X direction 832 | const Vector2d SteeredWheelBaseController::X_DIR = Vector2d::UnitX(); 833 | 834 | SteeredWheelBaseController::SteeredWheelBaseController() 835 | { 836 | state_ = CONSTRUCTED; 837 | } 838 | 839 | bool SteeredWheelBaseController::initRequest(RobotHW *const robot_hw, 840 | NodeHandle& /* root_nh */, 841 | NodeHandle& ctrlr_nh, 842 | ClaimedResources& claimed_resources) 843 | { 844 | if (state_ != CONSTRUCTED) 845 | { 846 | ROS_ERROR("The steered-wheel base controller could not be created."); 847 | return false; 848 | } 849 | 850 | EffortJointInterface *const eff_joint_iface = 851 | robot_hw->get(); 852 | PositionJointInterface *const pos_joint_iface = 853 | robot_hw->get(); 854 | VelocityJointInterface *const vel_joint_iface = 855 | robot_hw->get(); 856 | 857 | if (eff_joint_iface != NULL) 858 | eff_joint_iface->clearClaims(); 859 | if (pos_joint_iface != NULL) 860 | pos_joint_iface->clearClaims(); 861 | if (vel_joint_iface != NULL) 862 | vel_joint_iface->clearClaims(); 863 | 864 | try 865 | { 866 | init(eff_joint_iface, pos_joint_iface, vel_joint_iface, ctrlr_nh); 867 | } 868 | catch (const std::exception& ex) 869 | { 870 | ROS_ERROR_STREAM(ex.what()); 871 | return false; 872 | } 873 | 874 | claimed_resources.clear(); 875 | addClaimedResources(eff_joint_iface, "hardware_interface::EffortJointInterface", claimed_resources); 876 | addClaimedResources(pos_joint_iface, "hardware_interface::PositionJointInterface", claimed_resources); 877 | addClaimedResources(vel_joint_iface, "hardware_interface::VelocityJointInterface", claimed_resources); 878 | 879 | state_ = INITIALIZED; 880 | return true; 881 | } 882 | 883 | string SteeredWheelBaseController::getHardwareInterfaceType() const 884 | { 885 | return ""; 886 | } 887 | 888 | void SteeredWheelBaseController::starting(const Time& time) 889 | { 890 | BOOST_FOREACH(Wheel & wheel, wheels_) 891 | wheel.initJoints(); 892 | 893 | last_lin_vel_ = Vector2d(0, 0); 894 | last_yaw_vel_ = 0; 895 | 896 | if (comp_odom_) 897 | { 898 | last_odom_x_ = odom_to_base_.translation().x(); 899 | last_odom_y_ = odom_to_base_.translation().y(); 900 | last_odom_yaw_ = atan2(odom_to_base_(1, 0), odom_to_base_(0, 0)); 901 | last_odom_pub_time_ = time; 902 | last_odom_tf_pub_time_ = time; 903 | } 904 | 905 | vel_cmd_.x_vel = 0; 906 | vel_cmd_.y_vel = 0; 907 | vel_cmd_.yaw_vel = 0; 908 | vel_cmd_.last_vel_cmd_time = time; 909 | vel_cmd_buf_.initRT(vel_cmd_); 910 | } 911 | 912 | void SteeredWheelBaseController::update(const Time& time, 913 | const Duration& period) 914 | { 915 | const double delta_t = period.toSec(); 916 | if (delta_t <= 0) 917 | return; 918 | 919 | vel_cmd_ = *(vel_cmd_buf_.readFromRT()); 920 | Vector2d desired_lin_vel; 921 | double desired_yaw_vel; 922 | if (!vel_cmd_timeout_enabled_ || 923 | time - vel_cmd_.last_vel_cmd_time <= vel_cmd_timeout_) 924 | { 925 | desired_lin_vel = Vector2d(vel_cmd_.x_vel, vel_cmd_.y_vel); 926 | desired_yaw_vel = vel_cmd_.yaw_vel; 927 | } 928 | else 929 | { 930 | // Too much time has elapsed since the last velocity command was received. 931 | // Stop the robot. 932 | desired_lin_vel.setZero(); 933 | desired_yaw_vel = 0; 934 | } 935 | const double inv_delta_t = 1 / delta_t; 936 | 937 | const Vector2d lin_vel = enforceLinLimits(desired_lin_vel, 938 | delta_t, inv_delta_t); 939 | const double yaw_vel = enforceYawLimits(desired_yaw_vel, 940 | delta_t, inv_delta_t); 941 | ctrlWheels(lin_vel, yaw_vel, period); 942 | if (comp_odom_) 943 | compOdometry(time, inv_delta_t); 944 | } 945 | 946 | void SteeredWheelBaseController::stopping(const Time& time) 947 | { 948 | BOOST_FOREACH(Wheel & wheel, wheels_) 949 | wheel.stop(); 950 | } 951 | 952 | // Initialize this steered-wheel base controller. 953 | void SteeredWheelBaseController:: 954 | init(EffortJointInterface *const eff_joint_iface, 955 | PositionJointInterface *const pos_joint_iface, 956 | VelocityJointInterface *const vel_joint_iface, 957 | NodeHandle& ctrlr_nh) 958 | { 959 | string robot_desc_name; 960 | ctrlr_nh.param("robot_description_name", robot_desc_name, 961 | DEF_ROBOT_DESC_NAME); 962 | urdf::Model urdf_model; 963 | if (!urdf_model.initParam(robot_desc_name)) 964 | throw runtime_error("The URDF data was not found."); 965 | KDL::Tree model_tree; 966 | if (!kdl_parser::treeFromUrdfModel(urdf_model, model_tree)) 967 | throw runtime_error("The kinematic tree could not be created."); 968 | 969 | string base_link; 970 | ctrlr_nh.param("base_link", base_link, DEF_BASE_LINK); 971 | double timeout; 972 | ctrlr_nh.param("cmd_vel_timeout", timeout, DEF_CMD_VEL_TIMEOUT); 973 | vel_cmd_timeout_enabled_ = timeout > 0; 974 | if (vel_cmd_timeout_enabled_) 975 | vel_cmd_timeout_.fromSec(timeout); 976 | 977 | ctrlr_nh.param("linear_speed_limit", lin_speed_limit_, DEF_LIN_SPEED_LIMIT); 978 | has_lin_speed_limit_ = lin_speed_limit_ >= 0; 979 | ctrlr_nh.param("linear_acceleration_limit", lin_accel_limit_, 980 | DEF_LIN_ACCEL_LIMIT); 981 | has_lin_accel_limit_ = lin_accel_limit_ >= 0; 982 | // For safety, a valid deceleration limit must be greater than zero. 983 | ctrlr_nh.param("linear_deceleration_limit", lin_decel_limit_, 984 | DEF_LIN_DECEL_LIMIT); 985 | has_lin_decel_limit_ = lin_decel_limit_ > 0; 986 | 987 | ctrlr_nh.param("yaw_speed_limit", yaw_speed_limit_, DEF_YAW_SPEED_LIMIT); 988 | has_yaw_speed_limit_ = yaw_speed_limit_ >= 0; 989 | ctrlr_nh.param("yaw_acceleration_limit", yaw_accel_limit_, 990 | DEF_YAW_ACCEL_LIMIT); 991 | has_yaw_accel_limit_ = yaw_accel_limit_ >= 0; 992 | // For safety, a valid deceleration limit must be greater than zero. 993 | ctrlr_nh.param("yaw_deceleration_limit", yaw_decel_limit_, 994 | DEF_YAW_DECEL_LIMIT); 995 | has_yaw_decel_limit_ = yaw_decel_limit_ > 0; 996 | 997 | ctrlr_nh.param("full_axle_speed_angle", hermite_offset_, 998 | DEF_FULL_AXLE_SPEED_ANG); 999 | if (hermite_offset_ < 0 || hermite_offset_ > M_PI) 1000 | throw runtime_error("full_axle_speed_angle must be in the range [0, pi]."); 1001 | double zero_axle_speed_ang; 1002 | ctrlr_nh.param("zero_axle_speed_angle", zero_axle_speed_ang, 1003 | DEF_ZERO_AXLE_SPEED_ANG); 1004 | if (zero_axle_speed_ang < 0 || zero_axle_speed_ang > M_PI) 1005 | throw runtime_error("zero_axle_speed_angle must be in the range [0, pi]."); 1006 | if (hermite_offset_ >= zero_axle_speed_ang) 1007 | { 1008 | throw runtime_error("full_axle_speed_angle must be less than " 1009 | "zero_axle_speed_angle."); 1010 | } 1011 | hermite_scale_ = 1 / (zero_axle_speed_ang - hermite_offset_); 1012 | 1013 | // Wheels 1014 | 1015 | XmlRpcValue wheel_param_list; 1016 | if (!ctrlr_nh.getParam("wheels", wheel_param_list)) 1017 | throw runtime_error("No wheels were specified."); 1018 | if (wheel_param_list.getType() != XmlRpcValue::TypeArray) 1019 | throw runtime_error("The specified list of wheels is invalid."); 1020 | if (wheel_param_list.size() < 2) 1021 | throw runtime_error("At least two wheels must be specified."); 1022 | 1023 | double wheel_dia_scale; 1024 | ctrlr_nh.param("wheel_diameter_scale", wheel_dia_scale, DEF_WHEEL_DIA_SCALE); 1025 | if (wheel_dia_scale <= 0) 1026 | { 1027 | throw runtime_error("The specified wheel diameter scale is less than or " 1028 | "equal to zero."); 1029 | } 1030 | 1031 | for (int i = 0; i < wheel_param_list.size(); i++) 1032 | { 1033 | XmlRpcValue& wheel_params = wheel_param_list[i]; 1034 | if (wheel_params.getType() != XmlRpcValue::TypeStruct) 1035 | throw runtime_error("The specified list of wheels is invalid."); 1036 | 1037 | if (!wheel_params.hasMember("steering_joint")) 1038 | throw runtime_error("A steering joint was not specified."); 1039 | XmlRpcValue& xml_steer_joint = wheel_params["steering_joint"]; 1040 | if (!xml_steer_joint.valid() || 1041 | xml_steer_joint.getType() != XmlRpcValue::TypeString) 1042 | { 1043 | throw runtime_error("An invalid steering joint was specified."); 1044 | } 1045 | const string steer_joint_name = xml_steer_joint; 1046 | const urdf::JointConstSharedPtr steer_joint = urdf_model.getJoint(steer_joint_name); 1047 | if (steer_joint == NULL) 1048 | { 1049 | throw runtime_error("Steering joint \"" + steer_joint_name + 1050 | "\" was not found in the URDF data."); 1051 | } 1052 | const string steer_link = steer_joint->child_link_name; 1053 | 1054 | if (!wheel_params.hasMember("axle_joint")) 1055 | throw runtime_error("An axle joint was not specified."); 1056 | XmlRpcValue& xml_axle_joint = wheel_params["axle_joint"]; 1057 | if (!xml_axle_joint.valid() || 1058 | xml_axle_joint.getType() != XmlRpcValue::TypeString) 1059 | { 1060 | throw runtime_error("An invalid axle joint was specified."); 1061 | } 1062 | const string axle_joint_name = xml_axle_joint; 1063 | 1064 | if (!wheel_params.hasMember("diameter")) 1065 | throw runtime_error("A wheel diameter was not specified."); 1066 | XmlRpcValue& xml_dia = wheel_params["diameter"]; 1067 | if (!xml_dia.valid()) 1068 | throw runtime_error("An invalid wheel diameter was specified."); 1069 | double dia; 1070 | switch (xml_dia.getType()) 1071 | { 1072 | case XmlRpcValue::TypeInt: 1073 | { 1074 | const int tmp = xml_dia; 1075 | dia = tmp; 1076 | } 1077 | break; 1078 | case XmlRpcValue::TypeDouble: 1079 | dia = xml_dia; 1080 | break; 1081 | default: 1082 | throw runtime_error("An invalid wheel diameter was specified."); 1083 | } 1084 | if (dia <= 0) 1085 | { 1086 | throw runtime_error("A specified wheel diameter is less than or " 1087 | "equal to zero."); 1088 | } 1089 | // Circumference 1090 | const double circ = (2 * M_PI) * (wheel_dia_scale * dia) / 2; 1091 | 1092 | wheels_.push_back(Wheel(model_tree, base_link, steer_link, 1093 | getJoint(steer_joint_name, true, 1094 | ctrlr_nh, urdf_model, 1095 | eff_joint_iface, pos_joint_iface, 1096 | vel_joint_iface), 1097 | getJoint(axle_joint_name, false, 1098 | ctrlr_nh, urdf_model, 1099 | eff_joint_iface, pos_joint_iface, 1100 | vel_joint_iface), circ)); 1101 | } 1102 | 1103 | // Odometry 1104 | double odom_pub_freq; 1105 | ctrlr_nh.param("odometry_publishing_frequency", odom_pub_freq, 1106 | DEF_ODOM_PUB_FREQ); 1107 | comp_odom_ = odom_pub_freq > 0; 1108 | if (comp_odom_) 1109 | { 1110 | odom_pub_period_ = Duration(1 / odom_pub_freq); 1111 | 1112 | double init_x, init_y, init_yaw; 1113 | ctrlr_nh.param("initial_x", init_x, DEF_INIT_X); 1114 | ctrlr_nh.param("initial_y", init_y, DEF_INIT_Y); 1115 | ctrlr_nh.param("initial_yaw", init_yaw, DEF_INIT_YAW); 1116 | init_odom_to_base_.setIdentity(); 1117 | init_odom_to_base_.rotate(clamp(init_yaw, -M_PI, M_PI)); 1118 | init_odom_to_base_.translation() = Vector2d(init_x, init_y); 1119 | odom_to_base_ = init_odom_to_base_; 1120 | odom_affine_.setIdentity(); 1121 | 1122 | wheel_pos_.resize(2, wheels_.size()); 1123 | for (size_t col = 0; col < wheels_.size(); col++) 1124 | wheel_pos_.col(col) = wheels_[col].pos(); 1125 | const Vector2d centroid = wheel_pos_.rowwise().mean(); 1126 | wheel_pos_.colwise() -= centroid; 1127 | neg_wheel_centroid_ = -centroid; 1128 | new_wheel_pos_.resize(wheels_.size(), 2); 1129 | 1130 | string odom_frame, base_frame; 1131 | ctrlr_nh.param("odometry_frame", odom_frame, DEF_ODOM_FRAME); 1132 | ctrlr_nh.param("base_frame", base_frame, DEF_BASE_FRAME); 1133 | odom_pub_.msg_.header.frame_id = odom_frame; 1134 | odom_pub_.msg_.child_frame_id = base_frame; 1135 | odom_pub_.msg_.pose.pose.position.z = 0; 1136 | odom_pub_.msg_.twist.twist.linear.z = 0; 1137 | odom_pub_.msg_.twist.twist.angular.x = 0; 1138 | odom_pub_.msg_.twist.twist.angular.y = 0; 1139 | odom_pub_.init(ctrlr_nh, "/odom", 1); 1140 | 1141 | odom_tf_pub_.msg_.transforms.resize(1); 1142 | geometry_msgs::TransformStamped& odom_tf_trans = 1143 | odom_tf_pub_.msg_.transforms[0]; 1144 | odom_tf_trans.header.frame_id = odom_pub_.msg_.header.frame_id; 1145 | odom_tf_trans.child_frame_id = odom_pub_.msg_.child_frame_id; 1146 | odom_tf_trans.transform.translation.z = 0; 1147 | odom_tf_pub_.init(ctrlr_nh, "/tf", 1); 1148 | } 1149 | 1150 | vel_cmd_sub_ = ctrlr_nh.subscribe("/cmd_vel", 1, 1151 | &SteeredWheelBaseController::velCmdCB, 1152 | this); 1153 | } 1154 | 1155 | // Velocity command callback 1156 | void SteeredWheelBaseController::velCmdCB(const TwistConstPtr& vel_cmd) 1157 | { 1158 | vel_cmd_.x_vel = vel_cmd->linear.x; 1159 | vel_cmd_.y_vel = vel_cmd->linear.y; 1160 | vel_cmd_.yaw_vel = vel_cmd->angular.z; 1161 | vel_cmd_.last_vel_cmd_time = Time::now(); 1162 | vel_cmd_buf_.writeFromNonRT(vel_cmd_); 1163 | } 1164 | 1165 | // Enforce linear motion limits. 1166 | Vector2d SteeredWheelBaseController:: 1167 | enforceLinLimits(const Vector2d& desired_vel, 1168 | const double delta_t, const double inv_delta_t) 1169 | { 1170 | Vector2d vel = desired_vel; 1171 | if (has_lin_speed_limit_) 1172 | { 1173 | const double vel_mag = vel.norm(); 1174 | if (vel_mag > lin_speed_limit_) 1175 | vel = (vel / vel_mag) * lin_speed_limit_; 1176 | } 1177 | 1178 | Vector2d accel = (vel - last_lin_vel_) * inv_delta_t; 1179 | 1180 | if (accel.dot(last_lin_vel_) >= 0) 1181 | { 1182 | // Acceleration 1183 | 1184 | if (has_lin_accel_limit_) 1185 | { 1186 | const double accel_mag = accel.norm(); 1187 | if (accel_mag > lin_accel_limit_) 1188 | { 1189 | accel = (accel / accel_mag) * lin_accel_limit_; 1190 | vel = last_lin_vel_ + accel * delta_t; 1191 | } 1192 | } 1193 | } 1194 | else 1195 | { 1196 | // Deceleration 1197 | 1198 | if (has_lin_decel_limit_) 1199 | { 1200 | const double accel_mag = accel.norm(); 1201 | if (accel_mag > lin_decel_limit_) 1202 | { 1203 | accel = (accel / accel_mag) * lin_decel_limit_; 1204 | vel = last_lin_vel_ + accel * delta_t; 1205 | } 1206 | } 1207 | if (vel.dot(last_lin_vel_) < 0) 1208 | vel = Vector2d(0, 0); 1209 | } 1210 | 1211 | last_lin_vel_ = vel; 1212 | return vel; 1213 | } 1214 | 1215 | double SteeredWheelBaseController::enforceYawLimits(const double desired_vel, 1216 | const double delta_t, 1217 | const double inv_delta_t) 1218 | { 1219 | double vel = desired_vel; 1220 | if (has_yaw_speed_limit_) 1221 | vel = clamp(vel, -yaw_speed_limit_, yaw_speed_limit_); 1222 | 1223 | double accel = (vel - last_yaw_vel_) * inv_delta_t; 1224 | 1225 | const double accel_sign = sign(accel); 1226 | const double last_yaw_vel_sign = sign(last_yaw_vel_); 1227 | if (accel_sign == last_yaw_vel_sign || last_yaw_vel_sign == 0) 1228 | { 1229 | // Acceleration 1230 | 1231 | if (has_yaw_accel_limit_ && fabs(accel) > yaw_accel_limit_) 1232 | { 1233 | accel = accel_sign * yaw_accel_limit_; 1234 | vel = last_yaw_vel_ + accel * delta_t; 1235 | } 1236 | } 1237 | else 1238 | { 1239 | // Deceleration 1240 | 1241 | if (has_yaw_decel_limit_ && fabs(accel) > yaw_decel_limit_) 1242 | { 1243 | accel = accel_sign * yaw_decel_limit_; 1244 | vel = last_yaw_vel_ + accel * delta_t; 1245 | } 1246 | if (sign(vel) != last_yaw_vel_sign) 1247 | vel = 0; 1248 | } 1249 | 1250 | last_yaw_vel_ = vel; 1251 | return vel; 1252 | } 1253 | 1254 | // Control the wheels. 1255 | void SteeredWheelBaseController::ctrlWheels(const Vector2d& lin_vel, 1256 | const double yaw_vel, 1257 | const Duration& period) 1258 | { 1259 | const double lin_speed = lin_vel.norm(); 1260 | 1261 | if (yaw_vel == 0) 1262 | { 1263 | if (lin_speed > 0) 1264 | { 1265 | // Point the wheels in the same direction. 1266 | 1267 | const Vector2d dir = lin_vel / lin_speed; 1268 | const double theta = 1269 | copysign(acos(dir.dot(SteeredWheelBaseController::X_DIR)), dir.y()); 1270 | 1271 | double min_speed_gain = 1; 1272 | BOOST_FOREACH(Wheel & wheel, wheels_) 1273 | { 1274 | const double speed_gain = 1275 | wheel.ctrlSteering(theta, period, hermite_scale_, hermite_offset_); 1276 | } 1277 | const double lin_speed_2 = min_speed_gain * lin_speed; 1278 | BOOST_FOREACH(Wheel & wheel, wheels_) 1279 | { 1280 | wheel.ctrlAxle(lin_speed_2, period); 1281 | } 1282 | } 1283 | else 1284 | { 1285 | // Stop wheel rotation. 1286 | BOOST_FOREACH(Wheel & wheel, wheels_) 1287 | { 1288 | wheel.ctrlSteering(period, hermite_scale_, hermite_offset_); 1289 | wheel.ctrlAxle(0, period); 1290 | } 1291 | } 1292 | } 1293 | else // The yaw velocity is nonzero. 1294 | { 1295 | // Align the wheels so that they are tangent to circles centered 1296 | // at "center". 1297 | 1298 | Vector2d center; 1299 | if (lin_speed > 0) 1300 | { 1301 | const Vector2d dir = lin_vel / lin_speed; 1302 | center = Vector2d(-dir.y(), dir.x()) * lin_speed / yaw_vel; 1303 | } 1304 | else 1305 | { 1306 | center.setZero(); 1307 | } 1308 | 1309 | std::vector radii; 1310 | double min_speed_gain = 1; 1311 | BOOST_FOREACH(Wheel & wheel, wheels_) 1312 | { 1313 | Vector2d vec = wheel.pos(); 1314 | vec -= center; 1315 | const double radius = vec.norm(); 1316 | radii.push_back(radius); 1317 | double theta; 1318 | if (radius > 0) 1319 | { 1320 | vec /= radius; 1321 | theta = 1322 | copysign(acos(vec.dot(SteeredWheelBaseController::X_DIR)), vec.y()) + 1323 | M_PI_2; 1324 | } 1325 | else 1326 | { 1327 | theta = 0; 1328 | } 1329 | 1330 | const double speed_gain = 1331 | wheel.ctrlSteering(theta, period, hermite_scale_, hermite_offset_); 1332 | } 1333 | 1334 | const double lin_speed_gain = min_speed_gain * yaw_vel; 1335 | size_t i = 0; 1336 | BOOST_FOREACH(Wheel & wheel, wheels_) 1337 | { 1338 | wheel.ctrlAxle(lin_speed_gain * radii[i++], period); 1339 | } 1340 | } 1341 | } 1342 | 1343 | // Compute odometry. 1344 | void SteeredWheelBaseController::compOdometry(const Time& time, 1345 | const double inv_delta_t) 1346 | { 1347 | // Compute the rigid transform from wheel_pos_ to new_wheel_pos_. 1348 | 1349 | for (size_t row = 0; row < wheels_.size(); row++) 1350 | new_wheel_pos_.row(row) = wheels_[row].pos() + wheels_[row].getDeltaPos(); 1351 | const Eigen::RowVector2d new_wheel_centroid = 1352 | new_wheel_pos_.colwise().mean(); 1353 | new_wheel_pos_.rowwise() -= new_wheel_centroid; 1354 | 1355 | const Matrix2d h = wheel_pos_ * new_wheel_pos_; 1356 | const Eigen::JacobiSVD svd(h, Eigen::ComputeFullU | 1357 | Eigen::ComputeFullV); 1358 | Matrix2d rot = svd.matrixV() * svd.matrixU().transpose(); 1359 | if (rot.determinant() < 0) 1360 | rot.col(1) *= -1; 1361 | 1362 | odom_affine_.matrix().block(0, 0, 2, 2) = rot; 1363 | odom_affine_.translation() = 1364 | rot * neg_wheel_centroid_ + new_wheel_centroid.transpose(); 1365 | odom_to_base_ = odom_to_base_ * odom_affine_; 1366 | 1367 | const double odom_x = odom_to_base_.translation().x(); 1368 | const double odom_y = odom_to_base_.translation().y(); 1369 | const double odom_yaw = atan2(odom_to_base_(1, 0), odom_to_base_(0, 0)); 1370 | 1371 | // Publish the odometry. 1372 | 1373 | geometry_msgs::Quaternion orientation; 1374 | bool orientation_comped = false; 1375 | 1376 | // tf 1377 | if (time - last_odom_tf_pub_time_ >= odom_pub_period_ && 1378 | odom_tf_pub_.trylock()) 1379 | { 1380 | orientation = tf::createQuaternionMsgFromYaw(odom_yaw); 1381 | orientation_comped = true; 1382 | 1383 | geometry_msgs::TransformStamped& odom_tf_trans = 1384 | odom_tf_pub_.msg_.transforms[0]; 1385 | odom_tf_trans.header.stamp = time; 1386 | odom_tf_trans.transform.translation.x = odom_x; 1387 | odom_tf_trans.transform.translation.y = odom_y; 1388 | odom_tf_trans.transform.rotation = orientation; 1389 | 1390 | odom_tf_pub_.unlockAndPublish(); 1391 | last_odom_tf_pub_time_ = time; 1392 | } 1393 | 1394 | // odom 1395 | if (time - last_odom_pub_time_ >= odom_pub_period_ && odom_pub_.trylock()) 1396 | { 1397 | if (!orientation_comped) 1398 | orientation = tf::createQuaternionMsgFromYaw(odom_yaw); 1399 | 1400 | odom_pub_.msg_.header.stamp = time; 1401 | odom_pub_.msg_.pose.pose.position.x = odom_x; 1402 | odom_pub_.msg_.pose.pose.position.y = odom_y; 1403 | odom_pub_.msg_.pose.pose.orientation = orientation; 1404 | 1405 | odom_pub_.msg_.twist.twist.linear.x = 1406 | (odom_x - last_odom_x_) * inv_delta_t; 1407 | odom_pub_.msg_.twist.twist.linear.y = 1408 | (odom_y - last_odom_y_) * inv_delta_t; 1409 | odom_pub_.msg_.twist.twist.angular.z = 1410 | (odom_yaw - last_odom_yaw_) * inv_delta_t; 1411 | 1412 | odom_pub_.unlockAndPublish(); 1413 | last_odom_pub_time_ = time; 1414 | } 1415 | 1416 | last_odom_x_ = odom_x; 1417 | last_odom_y_ = odom_y; 1418 | last_odom_yaw_ = odom_yaw; 1419 | } 1420 | 1421 | } // namespace steered_wheel_base_controller 1422 | 1423 | PLUGINLIB_EXPORT_CLASS(steered_wheel_base_controller::SteeredWheelBaseController, controller_interface::ControllerBase) 1424 | -------------------------------------------------------------------------------- /youbot_gazebo_control/youbot_gazebo_control_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 | 8 | Copyright (c) 2013 Wunderkammer Laboratory 9 | Licensed under the Apache License, Version 2.0 (the "License"); 10 | you may not use this file except in compliance with the License. 11 | You may obtain a copy of the License at 12 | 13 | http://www.apache.org/licenses/LICENSE-2.0 14 | 15 | Unless required by applicable law or agreed to in writing, software 16 | distributed under the License is distributed on an "AS IS" BASIS, 17 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | See the License for the specific language governing permissions and 19 | limitations under the License. 20 | 21 | SteeredWheelBaseController is a base controller for mobile robots. 22 | It works with bases that have two or more independently-steerable driven 23 | wheels and zero or more omnidirectional passive wheels (e.g. swivel 24 | casters). 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /youbot_gazebo_robot/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(youbot_gazebo_robot) 3 | find_package(catkin REQUIRED) 4 | 5 | catkin_package( 6 | CATKIN_DEPENDS 7 | gazebo_ros 8 | robot_state_publisher 9 | youbot_gazebo_control 10 | youbot_gazebo_worlds 11 | ) 12 | 13 | 14 | ### TESTS 15 | if(CATKIN_ENABLE_TESTING) 16 | find_package(roslaunch REQUIRED) 17 | 18 | roslaunch_add_file_check(launch) 19 | endif() 20 | 21 | ### INSTALLS 22 | install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) 23 | -------------------------------------------------------------------------------- /youbot_gazebo_robot/launch/vel_youbot_arm.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /youbot_gazebo_robot/launch/youbot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 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 | -------------------------------------------------------------------------------- /youbot_gazebo_robot/launch/youbot_arm_only.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /youbot_gazebo_robot/launch/youbot_base_only.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /youbot_gazebo_robot/launch/youbot_dual_arm.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 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 | -------------------------------------------------------------------------------- /youbot_gazebo_robot/launch/youbot_with_cam3d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /youbot_gazebo_robot/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | youbot_gazebo_robot 4 | 0.1.0 5 | Launch the KUKA youBot in the Gazebo simulation 6 | 7 | Walter Nowak 8 | Sebastian Blumenthal 9 | 10 | GPLv3 11 | 12 | http://github.com/mas-group/youbot_simulation --> 13 | 14 | Frederik Hegger 15 | 16 | catkin 17 | 18 | gazebo_ros 19 | robot_state_publisher 20 | youbot_gazebo_control 21 | youbot_gazebo_worlds 22 | 23 | roslaunch 24 | 25 | 26 | -------------------------------------------------------------------------------- /youbot_gazebo_worlds/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(youbot_gazebo_worlds) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package( 7 | CATKIN_DEPENDS 8 | gazebo_ros 9 | ) 10 | 11 | 12 | ### TESTS 13 | if(CATKIN_ENABLE_TESTING) 14 | find_package(roslaunch REQUIRED) 15 | 16 | roslaunch_add_file_check(launch) 17 | endif() 18 | 19 | 20 | ### INSTALLS 21 | install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) 22 | install(DIRECTORY urdf/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf) 23 | -------------------------------------------------------------------------------- /youbot_gazebo_worlds/launch/empty_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /youbot_gazebo_worlds/launch/robocup_at_work_2012.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /youbot_gazebo_worlds/launch/tower_of_hanoi.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /youbot_gazebo_worlds/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | youbot_gazebo_worlds 4 | 0.0.0 5 | Gazebo worlds configurations 6 | 7 | Walter Nowak 8 | Sebastian Blumenthal 9 | 10 | GPLv3 11 | 12 | http://github.com/mas-group/youbot_simulation --> 13 | 14 | Frederik Hegger 15 | 16 | catkin 17 | 18 | gazebo_ros 19 | 20 | roslaunch 21 | 22 | 23 | -------------------------------------------------------------------------------- /youbot_gazebo_worlds/urdf/robocup_at_work_2012.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 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 | 158 | 159 | 160 | 161 | 162 | 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 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | Gazebo/Gray 262 | true 263 | 264 | 265 | Gazebo/Gray 266 | true 267 | 268 | 269 | Gazebo/Gray 270 | true 271 | 272 | 273 | Gazebo/Gray 274 | true 275 | 276 | 277 | Gazebo/Gray 278 | true 279 | 280 | 281 | Gazebo/Gray 282 | true 283 | 284 | 285 | Gazebo/Gray 286 | true 287 | 288 | 289 | Gazebo/Gray 290 | true 291 | 292 | 293 | Gazebo/Gray 294 | true 295 | 296 | 297 | Gazebo/Gray 298 | true 299 | 300 | 301 | 302 | -------------------------------------------------------------------------------- /youbot_gazebo_worlds/urdf/tower_of_hanoi.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | Gazebo/Red 148 | true 149 | 150 | 151 | Gazebo/Blue 152 | true 153 | 154 | 155 | Gazebo/Blue 156 | true 157 | 158 | 159 | Gazebo/Blue 160 | true 161 | 162 | 163 | Gazebo/Red 164 | true 165 | 166 | 167 | Gazebo/Red 168 | true 169 | 170 | 171 | 172 | 173 | -------------------------------------------------------------------------------- /youbot_simulation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(youbot_simulation) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /youbot_simulation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | youbot_simulation 4 | 0.1.0 5 | Meta packages which holds packages to run the KUKA youBot in the Gazebo simulation with ROS 6 | 7 | Walter Nowak 8 | Sebastian Blumenthal 9 | 10 | GPLv3 11 | 12 | http://github.com/mas-group/youbot_simulation --> 13 | 14 | Frederik Hegger 15 | 16 | catkin 17 | 18 | 19 | 20 | 21 | 22 | --------------------------------------------------------------------------------