├── .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 |
--------------------------------------------------------------------------------