add apm
This commit is contained in:
Binary file not shown.
|
After Width: | Height: | Size: 117 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 151 KiB |
@@ -0,0 +1,79 @@
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
Changelog for package diff_drive_controller
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
0.9.3 (2016-02-12)
|
||||
------------------
|
||||
* Reduced pedantry, redundancy.
|
||||
* Added tests for the odom_frame_id parameter.
|
||||
* Parameterized diff_drive_controller's odom_frame_id
|
||||
* add check for multiple publishers on cmd_vel
|
||||
* Address -Wunused-parameter warnings
|
||||
* Limit jerk
|
||||
* Add param velocity_rolling_window_size
|
||||
* Minor fixes
|
||||
1. Coding style
|
||||
2. Tolerance to fall-back to Runge-Kutta 2 integration
|
||||
3. Remove unused variables
|
||||
* Fix forward test
|
||||
Fix the following bugs in the testForward test:
|
||||
1. Check traveled distance in XY plane
|
||||
2. Use expected speed variable on test check
|
||||
* Add test for NaN
|
||||
* Add test for bad URDF
|
||||
This unit test exercises a controller load failure caused by
|
||||
a wrong wheel geometry. The controller requires that wheels be
|
||||
modeled by cylinders, while the bad URDF uses spheres.
|
||||
* Contributors: Adolfo Rodriguez Tsouroukdissian, Bence Magyar, Enrique Fernandez, Eric Tappan, Karsten Knese, Paul Mathieu, tappan-at-git
|
||||
|
||||
0.9.2 (2015-05-04)
|
||||
------------------
|
||||
* Allow the wheel separation and radius to be set from different sources
|
||||
i.e. one can be set from the URDF, the other from the parameter server.
|
||||
If wheel separation and wheel diameter is specified in the parameter server, don't look them up from urdf
|
||||
* Contributors: Bence Magyar, Nils Berg
|
||||
|
||||
0.9.1 (2014-11-03)
|
||||
------------------
|
||||
|
||||
0.9.0 (2014-10-31)
|
||||
------------------
|
||||
* Add support for multiple wheels per side
|
||||
* Odometry computation:
|
||||
- New option to compute in open loop fashion
|
||||
- New option to skip publishing odom frame to tf
|
||||
* Remove dependency on angles package
|
||||
* Buildsystem fixes
|
||||
* Contributors: Bence Magyar, Lukas Bulwahn, efernandez
|
||||
|
||||
0.8.1 (2014-07-11)
|
||||
------------------
|
||||
|
||||
0.8.0 (2014-05-12)
|
||||
------------------
|
||||
* Add base_frame_id param (defaults to base_link)
|
||||
The nav_msgs/Odometry message specifies the child_frame_id field,
|
||||
which was previously not set.
|
||||
This commit creates a parameter to replace the previously hard-coded
|
||||
value of the child_frame_id of the published tf frame, and uses it
|
||||
in the odom message as well.
|
||||
* Contributors: enriquefernandez
|
||||
|
||||
0.7.2 (2014-04-01)
|
||||
------------------
|
||||
|
||||
0.7.1 (2014-03-31)
|
||||
------------------
|
||||
* Changed test-depend to build-depend for release jobs.
|
||||
* Contributors: Bence Magyar
|
||||
|
||||
0.7.0 (2014-03-28)
|
||||
------------------
|
||||
* diff_drive_controller: New controller for differential drive wheel systems.
|
||||
* Control is in the form of a velocity command, that is split then sent on the two wheels of a differential drive
|
||||
wheel base.
|
||||
* Odometry is published to tf and to a dedicated nav__msgs/Odometry topic.
|
||||
* Realtime-safe implementation.
|
||||
* Implements task-space velocity and acceleration limits.
|
||||
* Automatic stop after command time-out.
|
||||
* Contributors: Bence Magyar, Paul Mathieu, Enrique Fernandez.
|
||||
103
Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/CMakeLists.txt
Executable file
103
Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/CMakeLists.txt
Executable file
@@ -0,0 +1,103 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(steer_drive_controller)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
controller_interface
|
||||
urdf
|
||||
realtime_tools
|
||||
tf
|
||||
nav_msgs
|
||||
roscpp
|
||||
angles
|
||||
control_toolbox
|
||||
gazebo_ros_control
|
||||
hardware_interface
|
||||
joint_limits_interface
|
||||
)
|
||||
|
||||
find_package(gazebo REQUIRED)
|
||||
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
LIBRARIES ${PROJECT_NAME}
|
||||
CATKIN_DEPENDS
|
||||
roscpp
|
||||
angles
|
||||
control_toolbox
|
||||
gazebo_ros_control
|
||||
hardware_interface
|
||||
joint_limits_interface
|
||||
)
|
||||
|
||||
include_directories(
|
||||
include ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
link_directories(${GAZEBO_LIBRARY_DIRS})
|
||||
list(APPEND CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
|
||||
|
||||
add_library(${PROJECT_NAME} src/steer_drive_controller.cpp src/odometry.cpp src/speed_limiter.cpp)
|
||||
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
|
||||
|
||||
install(TARGETS ${PROJECT_NAME}
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
|
||||
install(FILES steer_drive_controller_plugins.xml
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
|
||||
|
||||
if (CATKIN_ENABLE_TESTING)
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
rostest
|
||||
std_srvs
|
||||
controller_manager
|
||||
tf
|
||||
)
|
||||
include_directories(include ${catkin_INCLUDE_DIRS})
|
||||
|
||||
add_executable(${PROJECT_NAME}_steerbot test/common/src/steerbot.cpp)
|
||||
target_link_libraries(${PROJECT_NAME}_steerbot ${catkin_LIBRARIES})
|
||||
|
||||
add_dependencies(tests ${PROJECT_NAME}_steerbot)
|
||||
|
||||
add_rostest_gtest(${PROJECT_NAME}_test
|
||||
test/steer_drive_controller_test/steer_drive_controller.test
|
||||
test/steer_drive_controller_test/steer_drive_controller_test.cpp)
|
||||
target_link_libraries(${PROJECT_NAME}_test ${catkin_LIBRARIES})
|
||||
add_rostest_gtest(${PROJECT_NAME}_nan_test
|
||||
test/steer_drive_controller_nan_test/steer_drive_controller_nan.test
|
||||
test/steer_drive_controller_nan_test/steer_drive_controller_nan_test.cpp)
|
||||
target_link_libraries(${PROJECT_NAME}_nan_test ${catkin_LIBRARIES})
|
||||
add_rostest_gtest(${PROJECT_NAME}_limits_test
|
||||
test/steer_drive_controller_limits_test/steer_drive_controller_limits.test
|
||||
test/steer_drive_controller_limits_test/steer_drive_controller_limits_test.cpp)
|
||||
target_link_libraries(${PROJECT_NAME}_limits_test ${catkin_LIBRARIES})
|
||||
add_rostest_gtest(${PROJECT_NAME}_timeout_test
|
||||
test/steer_drive_controller_timeout_test/steer_drive_controller_timeout.test
|
||||
test/steer_drive_controller_timeout_test/steer_drive_controller_timeout_test.cpp)
|
||||
target_link_libraries(${PROJECT_NAME}_timeout_test ${catkin_LIBRARIES})
|
||||
add_rostest_gtest(steer_drive_controller_fail_test
|
||||
test/steer_drive_controller_fail_test/steer_drive_controller_wrong.test
|
||||
test/steer_drive_controller_fail_test/steer_drive_controller_fail_test.cpp)
|
||||
target_link_libraries(steer_drive_controller_fail_test ${catkin_LIBRARIES})
|
||||
add_rostest_gtest(${PROJECT_NAME}_odom_tf_test
|
||||
test/steer_drive_controller_odom_tf_test/steer_drive_controller_odom_tf.test
|
||||
test/steer_drive_controller_odom_tf_test/steer_drive_controller_odom_tf_test.cpp)
|
||||
target_link_libraries(${PROJECT_NAME}_odom_tf_test ${catkin_LIBRARIES})
|
||||
add_rostest_gtest(${PROJECT_NAME}_default_odom_frame_test
|
||||
test/steer_drive_controller_default_odom_frame_test/steer_drive_controller_default_odom_frame.test
|
||||
test/steer_drive_controller_default_odom_frame_test/steer_drive_controller_default_odom_frame_test.cpp)
|
||||
target_link_libraries(${PROJECT_NAME}_default_odom_frame_test ${catkin_LIBRARIES})
|
||||
add_rostest_gtest(steer_drive_controller_odom_frame_test
|
||||
test/steer_drive_controller_odom_frame_test/steer_drive_controller_odom_frame.test
|
||||
test/steer_drive_controller_odom_frame_test/steer_drive_controller_odom_frame_test.cpp)
|
||||
target_link_libraries(${PROJECT_NAME}_odom_frame_test ${catkin_LIBRARIES})
|
||||
add_rostest(test/steer_drive_controller_open_loop_test/steer_drive_controller_open_loop.test)
|
||||
add_rostest(test/steer_drive_controller_no_wheel_test/steer_drive_controller_no_wheel.test)
|
||||
add_rostest(test/steer_drive_controller_no_steer_test/steer_drive_controller_no_steer.test)
|
||||
add_rostest(test/steer_drive_controller_radius_param_test/steer_drive_controller_radius_param.test)
|
||||
add_rostest(test/steer_drive_controller_radius_param_fail_test/steer_drive_controller_radius_param_fail.test)
|
||||
add_rostest(test/steer_drive_controller_separation_param_test/steer_drive_controller_separation_param.test)
|
||||
endif()
|
||||
80
Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/README.md
Executable file
80
Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/README.md
Executable file
@@ -0,0 +1,80 @@
|
||||
## Steer Drive Controller ##
|
||||
|
||||
Controller for a steer drive mobile base.
|
||||
|
||||
## 仕様
|
||||
- Subscribe
|
||||
- `steer_drive_controller/cmd_vel` にTwist型のメッセージを投げて下さい．
|
||||
- Publish
|
||||
- `steer_drive_controller/odom` を出すべきなのですが，未完成です．
|
||||
|
||||
- 例：SteerDriveController + SteerBot(RobotHWSim) で起動して，以下のメッセージで動く
|
||||
```bash
|
||||
rostopic pub -1 steer_drive_controller/cmd_vel geometry_msgs/Twist -- '[1.0, 0.0, 0.0]' '[0.0, 0.0, 0.5]'
|
||||
```
|
||||
|
||||
## third_robotのlinkメモ
|
||||
### ベース
|
||||
- base_link
|
||||
|
||||
### ステアリング
|
||||
- steer
|
||||
|
||||
### ホイール
|
||||
- left_front_wheel
|
||||
- left_rear_wheel
|
||||
- right_front_wheel
|
||||
- right_rear_wheel
|
||||
|
||||
### センサ
|
||||
- camera_link
|
||||
- front_bottom_lrf
|
||||
- front_top_lrf
|
||||
- rear_bottom_lrf
|
||||
|
||||
## third_robotのjointメモ
|
||||
### ホイール
|
||||
- base_to_left_front_wheel
|
||||
- base_to_left_rear_wheel
|
||||
- base_to_right_front_wheel
|
||||
- base_to_right_rear_wheel
|
||||
|
||||
### ステアリング
|
||||
- base_to_steer
|
||||
- base_to_steer_right: gazebo用のvirtual.
|
||||
- base_to_steer_left: gazebo用のvirtual.
|
||||
|
||||
## デバッグ開発の手順
|
||||
### 準備
|
||||
- パスを通す
|
||||
|
||||
```bash
|
||||
source path.bash
|
||||
```
|
||||
|
||||
- gazeboを実行する
|
||||
```bash
|
||||
roslaunch third_robot_gazebo third_robot_world.launch
|
||||
```
|
||||
|
||||
- コントローラ動作用の最低限の設定を行うlaunchファイルを実行する
|
||||
```bash
|
||||
roslaunch steer_drive_controller steer_drive_test_setting.launch
|
||||
```
|
||||
|
||||
### デバッガ
|
||||
- パスを通したコンソールでQtCreatorを起動する
|
||||
```bash
|
||||
qtcreator
|
||||
```
|
||||
|
||||
- [QtCreatorでROSのパッケージをビルド&デバッグ実行する](http://qiita.com/MoriKen/items/ea41e485929e0724d15e)にしたがってビルドする．実行オプションで`steer_drive_test`を選択する．
|
||||
|
||||
- `third_robot_control/steer_drive_controller/include/steer_drive_controller.h`内で`#define GUI_DEBUG`をアンコメントアウトする．
|
||||
- 53行目：`#define GUI_DEBUG // uncommentout when you use qtcreator for debugging` ってとこ．
|
||||
- デバッグ用のモジュールに名前空間がついてないので，無理やり付与する設定．
|
||||
|
||||
- `third_robot_control/steer_drive_controller/test/steer_drive_test.cpp` のmain関数内でブレイクポイントを置いてデバッグ実行．
|
||||
- SteerRobotはフェイクで作ってある．
|
||||
- `bool rt_code = rb_controller.init(steer_drive_bot, nh_main, nh_control);` でコントローラの初期化
|
||||
|
||||
@@ -0,0 +1,4 @@
|
||||
# Publish all joint states -----------------------------------
|
||||
joint_state_controller:
|
||||
type: joint_state_controller/JointStateController
|
||||
publish_rate: 50
|
||||
@@ -0,0 +1,53 @@
|
||||
# -----------------------------------
|
||||
mobile_base_controller:
|
||||
type : "steer_drive_controller/SteerDriveController"
|
||||
rear_wheel: '$(arg prefix)steer2sd_wheel_joint'
|
||||
front_steer: '$(arg prefix)base2steer_joint'
|
||||
publish_rate: 41.2 # this is what the real MiR platform publishes (default: 50)
|
||||
|
||||
pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
|
||||
twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
|
||||
enable_odom_tf: false
|
||||
|
||||
# Wheel separation and diameter. These are both optional.
|
||||
# diff_drive_controller will attempt to read either one or both from the
|
||||
# URDF if not specified as a parameter.
|
||||
# We don't set the value here because it's different for each MiR type (100, 250, ...), and
|
||||
# the plugin figures out the correct values.
|
||||
wheel_separation : 1.3268
|
||||
wheel_radius : 0.125
|
||||
|
||||
# Wheel separation and radius multipliers
|
||||
wheel_separation_multiplier: 1.0 # default: 1.0
|
||||
wheel_radius_multiplier : 1.0 # default: 1.0
|
||||
|
||||
# Steer position angle multipliers for fine tuning.
|
||||
steer_pos_multiplier : 1.0
|
||||
|
||||
# Velocity commands timeout [s], default 0.5
|
||||
cmd_vel_timeout: 0.25
|
||||
|
||||
# frame_ids (same as real MiR platform)
|
||||
base_frame_id: $(arg prefix)base_footprint # default: base_link
|
||||
odom_frame_id: $(arg prefix)odom # default: odom
|
||||
|
||||
# Velocity and acceleration limits
|
||||
# Whenever a min_* is unspecified, default to -max_*
|
||||
linear:
|
||||
x:
|
||||
has_velocity_limits : true
|
||||
max_velocity : 1.0 # m/s
|
||||
min_velocity : -0.5 # m/s
|
||||
has_acceleration_limits: true
|
||||
max_acceleration : 1.38 # m/s^2
|
||||
min_acceleration : -1.38 # m/s^2
|
||||
has_jerk_limits : true
|
||||
max_jerk : 5.0 # m/s^3
|
||||
angular:
|
||||
z:
|
||||
has_velocity_limits : true
|
||||
max_velocity : 1.5 # rad/s
|
||||
has_acceleration_limits: true
|
||||
max_acceleration : 0.857142857 # rad/s^2
|
||||
has_jerk_limits : true
|
||||
max_jerk : 2.5 # rad/s^3
|
||||
@@ -0,0 +1,209 @@
|
||||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2013, PAL Robotics, S.L.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the PAL Robotics nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
|
||||
/*
|
||||
* Author: Luca Marchionni
|
||||
* Author: Bence Magyar
|
||||
* Author: Enrique Fernández
|
||||
* Author: Paul Mathieu
|
||||
*/
|
||||
|
||||
#ifndef ODOMETRY_H_
|
||||
#define ODOMETRY_H_
|
||||
|
||||
#include <ros/time.h>
|
||||
#include <boost/accumulators/accumulators.hpp>
|
||||
#include <boost/accumulators/statistics/stats.hpp>
|
||||
#include <boost/accumulators/statistics/rolling_mean.hpp>
|
||||
#include <boost/function.hpp>
|
||||
|
||||
namespace steer_drive_controller
|
||||
{
|
||||
namespace bacc = boost::accumulators;
|
||||
|
||||
/**
|
||||
* \brief The Odometry class handles odometry readings
|
||||
* (2D pose and velocity with related timestamp)
|
||||
*/
|
||||
class Odometry
|
||||
{
|
||||
public:
|
||||
|
||||
/// Integration function, used to integrate the odometry:
|
||||
typedef boost::function<void(double, double)> IntegrationFunction;
|
||||
|
||||
/**
|
||||
* \brief Constructor
|
||||
* Timestamp will get the current time value
|
||||
* Value will be set to zero
|
||||
* \param velocity_rolling_window_size Rolling window size used to compute the velocity mean
|
||||
*/
|
||||
Odometry(size_t velocity_rolling_window_size = 10);
|
||||
|
||||
/**
|
||||
* \brief Initialize the odometry
|
||||
* \param time Current time
|
||||
*/
|
||||
void init(const ros::Time &time);
|
||||
|
||||
/**
|
||||
* \brief Updates the odometry class with latest wheels position
|
||||
* \param rear_wheel_pos Rear wheel position [rad]
|
||||
* \param front_steer_pos Front Steer position [rad]
|
||||
* \param time Current time
|
||||
* \return true if the odometry is actually updated
|
||||
*/
|
||||
bool update(double rear_wheel_pos, double front_steer_pos, const ros::Time &time);
|
||||
|
||||
/**
|
||||
* \brief Updates the odometry class with latest velocity command
|
||||
* \param linear Linear velocity [m/s]
|
||||
* \param angular Angular velocity [rad/s]
|
||||
* \param time Current time
|
||||
*/
|
||||
void updateOpenLoop(double linear, double angular, const ros::Time &time);
|
||||
|
||||
/**
|
||||
* \brief heading getter
|
||||
* \return heading [rad]
|
||||
*/
|
||||
double getHeading() const
|
||||
{
|
||||
return heading_;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief x position getter
|
||||
* \return x position [m]
|
||||
*/
|
||||
double getX() const
|
||||
{
|
||||
return x_;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief y position getter
|
||||
* \return y position [m]
|
||||
*/
|
||||
double getY() const
|
||||
{
|
||||
return y_;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief linear velocity getter
|
||||
* \return linear velocity [m/s]
|
||||
*/
|
||||
double getLinear() const
|
||||
{
|
||||
return linear_;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief angular velocity getter
|
||||
* \return angular velocity [rad/s]
|
||||
*/
|
||||
double getAngular() const
|
||||
{
|
||||
return angular_;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Sets the wheel parameters: radius and separation
|
||||
* \param wheel_separation Seperation between left and right wheels [m]
|
||||
* \param wheel_radius Wheel radius [m]
|
||||
*/
|
||||
void setWheelParams(double wheel_reparation_h, double wheel_radius);
|
||||
|
||||
/**
|
||||
* \brief Velocity rolling window size setter
|
||||
* \param velocity_rolling_window_size Velocity rolling window size
|
||||
*/
|
||||
void setVelocityRollingWindowSize(size_t velocity_rolling_window_size);
|
||||
|
||||
private:
|
||||
|
||||
/// Rolling mean accumulator and window:
|
||||
typedef bacc::accumulator_set<double, bacc::stats<bacc::tag::rolling_mean> > RollingMeanAcc;
|
||||
typedef bacc::tag::rolling_window RollingWindow;
|
||||
|
||||
/**
|
||||
* \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta
|
||||
* \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by encoders
|
||||
* \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders
|
||||
*/
|
||||
void integrateRungeKutta2(double linear, double angular);
|
||||
|
||||
/**
|
||||
* \brief Integrates the velocities (linear and angular) using exact method
|
||||
* \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by encoders
|
||||
* \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders
|
||||
*/
|
||||
void integrateExact(double linear, double angular);
|
||||
|
||||
/**
|
||||
* \brief Reset linear and angular accumulators
|
||||
*/
|
||||
void resetAccumulators();
|
||||
|
||||
/// Current timestamp:
|
||||
ros::Time timestamp_;
|
||||
|
||||
/// Current pose:
|
||||
double x_; // [m]
|
||||
double y_; // [m]
|
||||
double heading_; // [rad]
|
||||
|
||||
/// Current velocity:
|
||||
double linear_; // [m/s]
|
||||
double angular_; // [rad/s]
|
||||
|
||||
/// Wheel kinematic parameters [m]:
|
||||
double wheel_separation_h_;
|
||||
double wheel_radius_;
|
||||
|
||||
/// Previous wheel position/state [rad]:
|
||||
double rear_wheel_old_pos_;
|
||||
|
||||
/// Rolling mean accumulators for the linar and angular velocities:
|
||||
size_t velocity_rolling_window_size_;
|
||||
RollingMeanAcc linear_acc_;
|
||||
RollingMeanAcc angular_acc_;
|
||||
|
||||
/// Integration funcion, used to integrate the odometry:
|
||||
IntegrationFunction integrate_fun_;
|
||||
};
|
||||
}
|
||||
|
||||
#endif /* ODOMETRY_H_ */
|
||||
@@ -0,0 +1,131 @@
|
||||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2013, PAL Robotics, S.L.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the PAL Robotics nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
|
||||
/*
|
||||
* Author: Enrique Fernández
|
||||
*/
|
||||
|
||||
#ifndef SPEED_LIMITER_H
|
||||
#define SPEED_LIMITER_H
|
||||
|
||||
namespace steer_drive_controller
|
||||
{
|
||||
|
||||
class SpeedLimiter
|
||||
{
|
||||
public:
|
||||
|
||||
/**
|
||||
* \brief Constructor
|
||||
* \param [in] has_velocity_limits if true, applies velocity limits
|
||||
* \param [in] has_acceleration_limits if true, applies acceleration limits
|
||||
* \param [in] has_jerk_limits if true, applies jerk limits
|
||||
* \param [in] min_velocity Minimum velocity [m/s], usually <= 0
|
||||
* \param [in] max_velocity Maximum velocity [m/s], usually >= 0
|
||||
* \param [in] min_acceleration Minimum acceleration [m/s^2], usually <= 0
|
||||
* \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0
|
||||
* \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0
|
||||
* \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0
|
||||
*/
|
||||
SpeedLimiter(
|
||||
bool has_velocity_limits = false,
|
||||
bool has_acceleration_limits = false,
|
||||
bool has_jerk_limits = false,
|
||||
double min_velocity = 0.0,
|
||||
double max_velocity = 0.0,
|
||||
double min_acceleration = 0.0,
|
||||
double max_acceleration = 0.0,
|
||||
double min_jerk = 0.0,
|
||||
double max_jerk = 0.0
|
||||
);
|
||||
|
||||
/**
|
||||
* \brief Limit the velocity and acceleration
|
||||
* \param [in, out] v Velocity [m/s]
|
||||
* \param [in] v0 Previous velocity to v [m/s]
|
||||
* \param [in] v1 Previous velocity to v0 [m/s]
|
||||
* \param [in] dt Time step [s]
|
||||
* \return Limiting factor (1.0 if none)
|
||||
*/
|
||||
double limit(double& v, double v0, double v1, double dt);
|
||||
|
||||
/**
|
||||
* \brief Limit the velocity
|
||||
* \param [in, out] v Velocity [m/s]
|
||||
* \return Limiting factor (1.0 if none)
|
||||
*/
|
||||
double limit_velocity(double& v);
|
||||
|
||||
/**
|
||||
* \brief Limit the acceleration
|
||||
* \param [in, out] v Velocity [m/s]
|
||||
* \param [in] v0 Previous velocity [m/s]
|
||||
* \param [in] dt Time step [s]
|
||||
* \return Limiting factor (1.0 if none)
|
||||
*/
|
||||
double limit_acceleration(double& v, double v0, double dt);
|
||||
|
||||
/**
|
||||
* \brief Limit the jerk
|
||||
* \param [in, out] v Velocity [m/s]
|
||||
* \param [in] v0 Previous velocity to v [m/s]
|
||||
* \param [in] v1 Previous velocity to v0 [m/s]
|
||||
* \param [in] dt Time step [s]
|
||||
* \return Limiting factor (1.0 if none)
|
||||
* \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control
|
||||
*/
|
||||
double limit_jerk(double& v, double v0, double v1, double dt);
|
||||
|
||||
public:
|
||||
// Enable/Disable velocity/acceleration/jerk limits:
|
||||
bool has_velocity_limits;
|
||||
bool has_acceleration_limits;
|
||||
bool has_jerk_limits;
|
||||
|
||||
// Velocity limits:
|
||||
double min_velocity;
|
||||
double max_velocity;
|
||||
|
||||
// Acceleration limits:
|
||||
double min_acceleration;
|
||||
double max_acceleration;
|
||||
|
||||
// Jerk limits:
|
||||
double min_jerk;
|
||||
double max_jerk;
|
||||
};
|
||||
|
||||
} // namespace diff_drive_controller
|
||||
|
||||
#endif // SPEED_LIMITER_H
|
||||
@@ -0,0 +1,225 @@
|
||||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2013, PAL Robotics, S.L.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the PAL Robotics nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
|
||||
/*
|
||||
* Author: Enrique Fernández
|
||||
*/
|
||||
|
||||
#include <controller_interface/controller.h>
|
||||
#include <controller_interface/multi_interface_controller.h>
|
||||
#include <hardware_interface/joint_command_interface.h>
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <tf/tfMessage.h>
|
||||
|
||||
#include <realtime_tools/realtime_buffer.h>
|
||||
#include <realtime_tools/realtime_publisher.h>
|
||||
|
||||
#include <steer_drive_controller/odometry.h>
|
||||
#include <steer_drive_controller/speed_limiter.h>
|
||||
|
||||
//#define GUI_DEBUG // uncommentout when you use qtcreator for debugging
|
||||
|
||||
namespace steer_drive_controller{
|
||||
|
||||
/**
|
||||
* This class makes some assumptions on the model of the robot:
|
||||
* - the rotation axes of wheels are collinear
|
||||
* - the wheels are identical in radius
|
||||
* Additional assumptions to not duplicate information readily available in the URDF:
|
||||
* - the wheels have the same parent frame
|
||||
* - a wheel collision geometry is a cylinder in the urdf
|
||||
* - a wheel joint frame center's vertical projection on the floor must lie within the contact patch
|
||||
*/
|
||||
class SteerDriveController
|
||||
: public controller_interface::MultiInterfaceController<
|
||||
hardware_interface::PositionJointInterface,
|
||||
hardware_interface::VelocityJointInterface>
|
||||
{
|
||||
// constants
|
||||
private:
|
||||
enum INDX_WHEEL {
|
||||
INDX_WHEEL_FRNT = 0,
|
||||
INDX_WHEEL_MID = 1,
|
||||
INDX_WHEEL_BACK = 2,
|
||||
};
|
||||
enum INDX_STEER {
|
||||
INDX_STEER_FRNT = 0,
|
||||
INDX_STEER_BACK = 1,
|
||||
};
|
||||
// constant
|
||||
enum INDEX_WHEEL {
|
||||
INDEX_RIGHT = 0,
|
||||
INDEX_LEFT = 1
|
||||
};
|
||||
public:
|
||||
SteerDriveController();
|
||||
|
||||
/**
|
||||
* \brief Initialize controller
|
||||
* \param hw Velocity joint interface for the wheels
|
||||
* \param root_nh Node handle at root namespace
|
||||
* \param controller_nh Node handle inside the controller namespace
|
||||
*/
|
||||
bool init(//hardware_interface::VelocityJointInterface *hw,
|
||||
hardware_interface::RobotHW* robot_hw,
|
||||
ros::NodeHandle& root_nh,
|
||||
ros::NodeHandle &controller_nh);
|
||||
|
||||
/**
|
||||
* \brief Updates controller, i.e. computes the odometry and sets the new velocity commands
|
||||
* \param time Current time
|
||||
* \param period Time since the last called to update
|
||||
*/
|
||||
void update(const ros::Time& time, const ros::Duration& period);
|
||||
|
||||
/**
|
||||
* \brief Starts controller
|
||||
* \param time Current time
|
||||
*/
|
||||
void starting(const ros::Time& time);
|
||||
|
||||
/**
|
||||
* \brief Stops controller
|
||||
* \param time Current time
|
||||
*/
|
||||
void stopping(const ros::Time& /*time*/);
|
||||
|
||||
private:
|
||||
std::string name_;
|
||||
|
||||
/// Odometry related:
|
||||
ros::Duration publish_period_;
|
||||
ros::Time last_state_publish_time_;
|
||||
bool open_loop_;
|
||||
|
||||
/// Hardware handles:
|
||||
hardware_interface::JointHandle rear_wheel_joint_;
|
||||
hardware_interface::JointHandle front_steer_joint_;
|
||||
|
||||
/// Velocity command related:
|
||||
struct Commands
|
||||
{
|
||||
double lin;
|
||||
double ang;
|
||||
ros::Time stamp;
|
||||
|
||||
Commands() : lin(0.0), ang(0.0), stamp(0.0) {}
|
||||
};
|
||||
realtime_tools::RealtimeBuffer<Commands> command_;
|
||||
Commands command_struct_;
|
||||
ros::Subscriber sub_command_;
|
||||
|
||||
/// Odometry related:
|
||||
boost::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::Odometry> > odom_pub_;
|
||||
boost::shared_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > tf_odom_pub_;
|
||||
Odometry odometry_;
|
||||
|
||||
/// Wheel separation, wrt the midpoint of the wheel width:
|
||||
double wheel_separation_h_;
|
||||
|
||||
/// Wheel radius (assuming it's the same for the left and right wheels):
|
||||
double wheel_radius_;
|
||||
|
||||
/// Wheel separation and radius calibration multipliers:
|
||||
double wheel_separation_h_multiplier_;
|
||||
double wheel_radius_multiplier_;
|
||||
double steer_pos_multiplier_;
|
||||
|
||||
/// Timeout to consider cmd_vel commands old:
|
||||
double cmd_vel_timeout_;
|
||||
|
||||
/// Whether to allow multiple publishers on cmd_vel topic or not:
|
||||
bool allow_multiple_cmd_vel_publishers_;
|
||||
|
||||
/// Frame to use for the robot base:
|
||||
std::string base_frame_id_;
|
||||
|
||||
/// Frame to use for odometry and odom tf:
|
||||
std::string odom_frame_id_;
|
||||
|
||||
/// Whether to publish odometry to tf or not:
|
||||
bool enable_odom_tf_;
|
||||
|
||||
/// Number of wheel joints:
|
||||
size_t wheel_joints_size_;
|
||||
|
||||
/// Number of steer joints:
|
||||
size_t steer_joints_size_;
|
||||
|
||||
/// Speed limiters:
|
||||
Commands last1_cmd_;
|
||||
Commands last0_cmd_;
|
||||
SpeedLimiter limiter_lin_;
|
||||
SpeedLimiter limiter_ang_;
|
||||
|
||||
// FOR_DEBUG
|
||||
std::string ns_;
|
||||
|
||||
private:
|
||||
/**
|
||||
* \brief Brakes the wheels, i.e. sets the velocity to 0
|
||||
*/
|
||||
void brake();
|
||||
|
||||
/**
|
||||
* \brief Velocity command callback
|
||||
* \param command Velocity command message (twist)
|
||||
*/
|
||||
void cmdVelCallback(const geometry_msgs::Twist& command);
|
||||
|
||||
/**
|
||||
* \brief Sets odometry parameters from the URDF, i.e. the wheel radius and separation
|
||||
* \param root_nh Root node handle
|
||||
* \param left_wheel_name Name of the left wheel joint
|
||||
* \param right_wheel_name Name of the right wheel joint
|
||||
*/
|
||||
bool setOdomParamsFromUrdf(ros::NodeHandle& root_nh,
|
||||
const std::string rear_wheel_name,
|
||||
const std::string front_steer_name,
|
||||
bool lookup_wheel_separation_h,
|
||||
bool lookup_wheel_radius);
|
||||
|
||||
/**
|
||||
* \brief Sets the odometry publishing fields
|
||||
* \param root_nh Root node handle
|
||||
* \param controller_nh Node handle inside the controller namespace
|
||||
*/
|
||||
void setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh);
|
||||
|
||||
};
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(steer_drive_controller::SteerDriveController, controller_interface::ControllerBase)
|
||||
} // namespace diff_drive_controller
|
||||
43
Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/package.xml
Executable file
43
Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/package.xml
Executable file
@@ -0,0 +1,43 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>steer_drive_controller</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Controller for a steer drive mobile base.</description>
|
||||
<maintainer email="p595201m@mail.kyutech.jp">Masaru Morita</maintainer>
|
||||
|
||||
<license>BSD</license>
|
||||
|
||||
<url type="bugtracker">https://github.com/CIR-KIT/steer_drive_ros/issues</url>
|
||||
<url type="repository">https://github.com/CIR-KIT/steer_drive_ros.git</url>
|
||||
<author email="cirkit.infomation@gmail.com">CIR-KIT</author>
|
||||
<author email="p595201m@mail.kyutech.jp">Masaru Morita</author>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<depend>angles</depend>
|
||||
<depend>controller_interface</depend>
|
||||
<depend>control_toolbox</depend>
|
||||
<depend>gazebo_ros_control</depend>
|
||||
<depend>hardware_interface</depend>
|
||||
<depend>joint_limits_interface</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>realtime_tools</depend>
|
||||
<depend>roscpp</depend>
|
||||
<depend>tf</depend>
|
||||
<depend>urdf</depend>
|
||||
|
||||
<build_depend>gazebo</build_depend>
|
||||
|
||||
<!--Tests-->
|
||||
<build_depend>rostest</build_depend>
|
||||
|
||||
<test_depend>controller_manager</test_depend>
|
||||
<test_depend>rostest</test_depend>
|
||||
<test_depend>rosunit</test_depend>
|
||||
<test_depend>std_srvs</test_depend>
|
||||
<test_depend>xacro</test_depend>
|
||||
|
||||
<export>
|
||||
<controller_interface plugin="${prefix}/steer_drive_controller_plugins.xml"/>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,9 @@
|
||||
#!/bin/bash
|
||||
|
||||
# include path を作成する
|
||||
include_path=$(pwd)/include
|
||||
gazebo_path=/usr/include/gazebo-2.2/
|
||||
sdf_path=/usr/include/sdformat-1.4/
|
||||
|
||||
# CPATHに追加
|
||||
export CPATH=${CPATH}:${include_path}:${gazebo_path}:${sdf_path}
|
||||
@@ -0,0 +1,174 @@
|
||||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2013, PAL Robotics, S.L.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the PAL Robotics nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
|
||||
/*
|
||||
* Author: Luca Marchionni
|
||||
* Author: Bence Magyar
|
||||
* Author: Enrique Fernández
|
||||
* Author: Paul Mathieu
|
||||
*/
|
||||
|
||||
#include <steer_drive_controller/odometry.h>
|
||||
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
namespace steer_drive_controller
|
||||
{
|
||||
namespace bacc = boost::accumulators;
|
||||
|
||||
Odometry::Odometry(size_t velocity_rolling_window_size)
|
||||
: timestamp_(0.0)
|
||||
, x_(0.0)
|
||||
, y_(0.0)
|
||||
, heading_(0.0)
|
||||
, linear_(0.0)
|
||||
, angular_(0.0)
|
||||
, wheel_separation_h_(0.0)
|
||||
, wheel_radius_(0.0)
|
||||
, rear_wheel_old_pos_(0.0)
|
||||
, velocity_rolling_window_size_(velocity_rolling_window_size)
|
||||
, linear_acc_(RollingWindow::window_size = velocity_rolling_window_size)
|
||||
, angular_acc_(RollingWindow::window_size = velocity_rolling_window_size)
|
||||
, integrate_fun_(boost::bind(&Odometry::integrateExact, this, _1, _2))
|
||||
{
|
||||
}
|
||||
|
||||
void Odometry::init(const ros::Time& time)
|
||||
{
|
||||
// Reset accumulators and timestamp:
|
||||
resetAccumulators();
|
||||
timestamp_ = time;
|
||||
}
|
||||
|
||||
bool Odometry::update(double rear_wheel_pos, double front_steer_pos, const ros::Time &time)
|
||||
{
|
||||
/// Get current wheel joint positions:
|
||||
const double rear_wheel_cur_pos = rear_wheel_pos * wheel_radius_;
|
||||
|
||||
/// Estimate velocity of wheels using old and current position:
|
||||
//const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_;
|
||||
//const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_;
|
||||
|
||||
const double rear_wheel_est_vel = rear_wheel_cur_pos - rear_wheel_old_pos_;
|
||||
|
||||
/// Update old position with current:
|
||||
rear_wheel_old_pos_ = rear_wheel_cur_pos;
|
||||
|
||||
/// Compute linear and angular diff:
|
||||
const double linear = rear_wheel_est_vel * cos(front_steer_pos);//(right_wheel_est_vel + left_wheel_est_vel) * 0.5;
|
||||
//const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_w_;
|
||||
const double angular = tan(front_steer_pos) * linear / wheel_separation_h_;
|
||||
|
||||
/// Integrate odometry:
|
||||
integrate_fun_(linear, angular);
|
||||
|
||||
/// We cannot estimate the speed with very small time intervals:
|
||||
const double dt = (time - timestamp_).toSec();
|
||||
if (dt < 0.0001)
|
||||
return false; // Interval too small to integrate with
|
||||
|
||||
timestamp_ = time;
|
||||
|
||||
/// Estimate speeds using a rolling mean to filter them out:
|
||||
linear_acc_(linear/dt);
|
||||
angular_acc_(angular/dt);
|
||||
|
||||
linear_ = bacc::rolling_mean(linear_acc_);
|
||||
angular_ = bacc::rolling_mean(angular_acc_);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void Odometry::updateOpenLoop(double linear, double angular, const ros::Time &time)
|
||||
{
|
||||
/// Save last linear and angular velocity:
|
||||
linear_ = linear;
|
||||
angular_ = angular;
|
||||
|
||||
/// Integrate odometry:
|
||||
const double dt = (time - timestamp_).toSec();
|
||||
timestamp_ = time;
|
||||
integrate_fun_(linear * dt, angular * dt);
|
||||
}
|
||||
|
||||
void Odometry::setWheelParams(double wheel_separation_h, double wheel_radius)
|
||||
{
|
||||
wheel_separation_h_ = wheel_separation_h;
|
||||
wheel_radius_ = wheel_radius;
|
||||
}
|
||||
|
||||
void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
|
||||
{
|
||||
velocity_rolling_window_size_ = velocity_rolling_window_size;
|
||||
|
||||
resetAccumulators();
|
||||
}
|
||||
|
||||
void Odometry::integrateRungeKutta2(double linear, double angular)
|
||||
{
|
||||
const double direction = heading_ + angular * 0.5;
|
||||
|
||||
/// Runge-Kutta 2nd order integration:
|
||||
x_ += linear * cos(direction);
|
||||
y_ += linear * sin(direction);
|
||||
heading_ += angular;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Other possible integration method provided by the class
|
||||
* \param linear
|
||||
* \param angular
|
||||
*/
|
||||
void Odometry::integrateExact(double linear, double angular)
|
||||
{
|
||||
if (fabs(angular) < 1e-6)
|
||||
integrateRungeKutta2(linear, angular);
|
||||
else
|
||||
{
|
||||
/// Exact integration (should solve problems when angular is zero):
|
||||
const double heading_old = heading_;
|
||||
const double r = linear/angular;
|
||||
heading_ += angular;
|
||||
x_ += r * (sin(heading_) - sin(heading_old));
|
||||
y_ += -r * (cos(heading_) - cos(heading_old));
|
||||
}
|
||||
}
|
||||
|
||||
void Odometry::resetAccumulators()
|
||||
{
|
||||
linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
|
||||
angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
|
||||
}
|
||||
|
||||
} // namespace diff_drive_controller
|
||||
@@ -0,0 +1,137 @@
|
||||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2013, PAL Robotics, S.L.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the PAL Robotics nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
|
||||
/*
|
||||
* Author: Enrique Fernández
|
||||
*/
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
#include <steer_drive_controller/speed_limiter.h>
|
||||
|
||||
template<typename T>
|
||||
T clamp(T x, T min, T max)
|
||||
{
|
||||
return std::min(std::max(min, x), max);
|
||||
}
|
||||
|
||||
namespace steer_drive_controller
|
||||
{
|
||||
|
||||
SpeedLimiter::SpeedLimiter(
|
||||
bool has_velocity_limits,
|
||||
bool has_acceleration_limits,
|
||||
bool has_jerk_limits,
|
||||
double min_velocity,
|
||||
double max_velocity,
|
||||
double min_acceleration,
|
||||
double max_acceleration,
|
||||
double min_jerk,
|
||||
double max_jerk
|
||||
)
|
||||
: has_velocity_limits(has_velocity_limits)
|
||||
, has_acceleration_limits(has_acceleration_limits)
|
||||
, has_jerk_limits(has_jerk_limits)
|
||||
, min_velocity(min_velocity)
|
||||
, max_velocity(max_velocity)
|
||||
, min_acceleration(min_acceleration)
|
||||
, max_acceleration(max_acceleration)
|
||||
, min_jerk(min_jerk)
|
||||
, max_jerk(max_jerk)
|
||||
{
|
||||
}
|
||||
|
||||
double SpeedLimiter::limit(double& v, double v0, double v1, double dt)
|
||||
{
|
||||
const double tmp = v;
|
||||
|
||||
limit_jerk(v, v0, v1, dt);
|
||||
limit_acceleration(v, v0, dt);
|
||||
limit_velocity(v);
|
||||
|
||||
return tmp != 0.0 ? v / tmp : 1.0;
|
||||
}
|
||||
|
||||
double SpeedLimiter::limit_velocity(double& v)
|
||||
{
|
||||
const double tmp = v;
|
||||
|
||||
if (has_velocity_limits)
|
||||
{
|
||||
v = clamp(v, min_velocity, max_velocity);
|
||||
}
|
||||
|
||||
return tmp != 0.0 ? v / tmp : 1.0;
|
||||
}
|
||||
|
||||
double SpeedLimiter::limit_acceleration(double& v, double v0, double dt)
|
||||
{
|
||||
const double tmp = v;
|
||||
|
||||
if (has_acceleration_limits)
|
||||
{
|
||||
const double dv_min = min_acceleration * dt;
|
||||
const double dv_max = max_acceleration * dt;
|
||||
|
||||
const double dv = clamp(v - v0, dv_min, dv_max);
|
||||
|
||||
v = v0 + dv;
|
||||
}
|
||||
|
||||
return tmp != 0.0 ? v / tmp : 1.0;
|
||||
}
|
||||
|
||||
double SpeedLimiter::limit_jerk(double& v, double v0, double v1, double dt)
|
||||
{
|
||||
const double tmp = v;
|
||||
|
||||
if (has_jerk_limits)
|
||||
{
|
||||
const double dv = v - v0;
|
||||
const double dv0 = v0 - v1;
|
||||
|
||||
const double dt2 = 2. * dt * dt;
|
||||
|
||||
const double da_min = min_jerk * dt2;
|
||||
const double da_max = max_jerk * dt2;
|
||||
|
||||
const double da = clamp(dv - dv0, da_min, da_max);
|
||||
|
||||
v = v0 + dv0 + da;
|
||||
}
|
||||
|
||||
return tmp != 0.0 ? v / tmp : 1.0;
|
||||
}
|
||||
|
||||
} // namespace diff_drive_controller
|
||||
@@ -0,0 +1,493 @@
|
||||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2013, PAL Robotics, S.L.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the PAL Robotics nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
|
||||
/*
|
||||
* Author: Bence Magyar
|
||||
*/
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <tf/transform_datatypes.h>
|
||||
|
||||
#include <urdf_parser/urdf_parser.h>
|
||||
|
||||
#include <boost/assign.hpp>
|
||||
|
||||
#include <steer_drive_controller/steer_drive_controller.h>
|
||||
|
||||
static double euclideanOfVectors(const urdf::Vector3 &vec1, const urdf::Vector3 &vec2)
|
||||
{
|
||||
return std::sqrt(std::pow(vec1.x - vec2.x, 2) +
|
||||
std::pow(vec1.y - vec2.y, 2) +
|
||||
std::pow(vec1.z - vec2.z, 2));
|
||||
}
|
||||
|
||||
/*
|
||||
* \brief Check if the link is modeled as a cylinder
|
||||
* \param link Link
|
||||
* \return true if the link is modeled as a Cylinder; false otherwise
|
||||
*/
|
||||
static bool isCylinder(const urdf::LinkConstSharedPtr &link)
|
||||
{
|
||||
if (!link)
|
||||
{
|
||||
ROS_ERROR("Link == NULL.");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!link->collision)
|
||||
{
|
||||
ROS_ERROR_STREAM("Link " << link->name << " does not have collision description. Add collision description for link to urdf.");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!link->collision->geometry)
|
||||
{
|
||||
ROS_ERROR_STREAM("Link " << link->name << " does not have collision geometry description. Add collision geometry description for link to urdf.");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (link->collision->geometry->type != urdf::Geometry::CYLINDER)
|
||||
{
|
||||
ROS_ERROR_STREAM("Link " << link->name << " does not have cylinder geometry");
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/*
|
||||
* \brief Get the wheel radius
|
||||
* \param [in] wheel_link Wheel link
|
||||
* \param [out] wheel_radius Wheel radius [m]
|
||||
* \return true if the wheel radius was found; false other
|
||||
wise
|
||||
*/
|
||||
static bool getWheelRadius(const urdf::LinkConstSharedPtr &wheel_link, double &wheel_radius)
|
||||
{
|
||||
if (!isCylinder(wheel_link))
|
||||
{
|
||||
ROS_ERROR_STREAM("Wheel link " << wheel_link->name << " is NOT modeled as a cylinder!");
|
||||
return false;
|
||||
}
|
||||
|
||||
wheel_radius = (static_cast<urdf::Cylinder *>(wheel_link->collision->geometry.get()))->radius;
|
||||
return true;
|
||||
}
|
||||
|
||||
namespace steer_drive_controller
|
||||
{
|
||||
|
||||
SteerDriveController::SteerDriveController()
|
||||
: open_loop_(false), command_struct_(), wheel_separation_h_(0.0), wheel_radius_(0.0), wheel_separation_h_multiplier_(1.0), wheel_radius_multiplier_(1.0), steer_pos_multiplier_(1.0), cmd_vel_timeout_(0.5), allow_multiple_cmd_vel_publishers_(true), base_frame_id_("base_link"), odom_frame_id_("odom"), enable_odom_tf_(true), wheel_joints_size_(0), ns_("")
|
||||
{
|
||||
}
|
||||
|
||||
bool SteerDriveController::init(hardware_interface::RobotHW *robot_hw,
|
||||
ros::NodeHandle &root_nh,
|
||||
ros::NodeHandle &controller_nh)
|
||||
{
|
||||
typedef hardware_interface::VelocityJointInterface VelIface;
|
||||
typedef hardware_interface::PositionJointInterface PosIface;
|
||||
typedef hardware_interface::JointStateInterface StateIface;
|
||||
|
||||
// get multiple types of hardware_interface
|
||||
VelIface *vel_joint_if = robot_hw->get<VelIface>(); // vel for wheels
|
||||
PosIface *pos_joint_if = robot_hw->get<PosIface>(); // pos for steers
|
||||
|
||||
const std::string complete_ns = controller_nh.getNamespace();
|
||||
|
||||
std::size_t id = complete_ns.find_last_of("/");
|
||||
name_ = complete_ns.substr(id + 1);
|
||||
|
||||
//-- single rear wheel joint
|
||||
std::string rear_wheel_name = "rear_wheel_joint";
|
||||
controller_nh.param(ns_ + "rear_wheel", rear_wheel_name, rear_wheel_name);
|
||||
|
||||
//-- single front steer joint
|
||||
std::string front_steer_name = "front_steer_joint";
|
||||
controller_nh.param(ns_ + "front_steer", front_steer_name, front_steer_name);
|
||||
|
||||
// Odometry related:
|
||||
double publish_rate;
|
||||
controller_nh.param("publish_rate", publish_rate, 50.0);
|
||||
ROS_INFO_STREAM_NAMED(name_, "Controller state will be published at "
|
||||
<< publish_rate << "Hz.");
|
||||
publish_period_ = ros::Duration(1.0 / publish_rate);
|
||||
|
||||
controller_nh.param(ns_ + "open_loop", open_loop_, open_loop_);
|
||||
|
||||
controller_nh.param(ns_ + "wheel_separation_h_multiplier", wheel_separation_h_multiplier_, wheel_separation_h_multiplier_);
|
||||
ROS_INFO_STREAM_NAMED(name_, "Wheel separation height will be multiplied by "
|
||||
<< wheel_separation_h_multiplier_ << ".");
|
||||
|
||||
controller_nh.param(ns_ + "wheel_radius_multiplier", wheel_radius_multiplier_, wheel_radius_multiplier_);
|
||||
ROS_INFO_STREAM_NAMED(name_, "Wheel radius will be multiplied by "
|
||||
<< wheel_radius_multiplier_ << ".");
|
||||
|
||||
controller_nh.param(ns_ + "steer_pos_multiplier", steer_pos_multiplier_, steer_pos_multiplier_);
|
||||
ROS_INFO_STREAM_NAMED(name_, "Steer pos will be multiplied by "
|
||||
<< steer_pos_multiplier_ << ".");
|
||||
|
||||
int velocity_rolling_window_size = 10;
|
||||
controller_nh.param(ns_ + "velocity_rolling_window_size", velocity_rolling_window_size, velocity_rolling_window_size);
|
||||
ROS_INFO_STREAM_NAMED(name_, "Velocity rolling window size of "
|
||||
<< velocity_rolling_window_size << ".");
|
||||
|
||||
odometry_.setVelocityRollingWindowSize(velocity_rolling_window_size);
|
||||
|
||||
// Twist command related:
|
||||
controller_nh.param(ns_ + "cmd_vel_timeout", cmd_vel_timeout_, cmd_vel_timeout_);
|
||||
ROS_INFO_STREAM_NAMED(name_, "Velocity commands will be considered old if they are older than "
|
||||
<< cmd_vel_timeout_ << "s.");
|
||||
|
||||
controller_nh.param(ns_ + "allow_multiple_cmd_vel_publishers", allow_multiple_cmd_vel_publishers_, allow_multiple_cmd_vel_publishers_);
|
||||
ROS_INFO_STREAM_NAMED(name_, "Allow mutiple cmd_vel publishers is "
|
||||
<< (allow_multiple_cmd_vel_publishers_ ? "enabled" : "disabled"));
|
||||
|
||||
controller_nh.param(ns_ + "base_frame_id", base_frame_id_, base_frame_id_);
|
||||
ROS_INFO_STREAM_NAMED(name_, "Base frame_id set to " << base_frame_id_);
|
||||
|
||||
controller_nh.param(ns_ + "odom_frame_id", odom_frame_id_, odom_frame_id_);
|
||||
ROS_INFO_STREAM_NAMED(name_, "Odometry frame_id set to " << odom_frame_id_);
|
||||
|
||||
controller_nh.param(ns_ + "enable_odom_tf", enable_odom_tf_, enable_odom_tf_);
|
||||
ROS_INFO_STREAM_NAMED(name_, "Publishing to tf is " << (enable_odom_tf_ ? "enabled" : "disabled"));
|
||||
|
||||
// Velocity and acceleration limits:
|
||||
controller_nh.param(ns_ + "linear/x/has_velocity_limits", limiter_lin_.has_velocity_limits, limiter_lin_.has_velocity_limits);
|
||||
controller_nh.param(ns_ + "linear/x/has_acceleration_limits", limiter_lin_.has_acceleration_limits, limiter_lin_.has_acceleration_limits);
|
||||
controller_nh.param(ns_ + "linear/x/has_jerk_limits", limiter_lin_.has_jerk_limits, limiter_lin_.has_jerk_limits);
|
||||
controller_nh.param(ns_ + "linear/x/max_velocity", limiter_lin_.max_velocity, limiter_lin_.max_velocity);
|
||||
controller_nh.param(ns_ + "linear/x/min_velocity", limiter_lin_.min_velocity, -limiter_lin_.max_velocity);
|
||||
controller_nh.param(ns_ + "linear/x/max_acceleration", limiter_lin_.max_acceleration, limiter_lin_.max_acceleration);
|
||||
controller_nh.param(ns_ + "linear/x/min_acceleration", limiter_lin_.min_acceleration, -limiter_lin_.max_acceleration);
|
||||
controller_nh.param(ns_ + "linear/x/max_jerk", limiter_lin_.max_jerk, limiter_lin_.max_jerk);
|
||||
controller_nh.param(ns_ + "linear/x/min_jerk", limiter_lin_.min_jerk, -limiter_lin_.max_jerk);
|
||||
|
||||
controller_nh.param(ns_ + "angular/z/has_velocity_limits", limiter_ang_.has_velocity_limits, limiter_ang_.has_velocity_limits);
|
||||
controller_nh.param(ns_ + "angular/z/has_acceleration_limits", limiter_ang_.has_acceleration_limits, limiter_ang_.has_acceleration_limits);
|
||||
controller_nh.param(ns_ + "angular/z/has_jerk_limits", limiter_ang_.has_jerk_limits, limiter_ang_.has_jerk_limits);
|
||||
controller_nh.param(ns_ + "angular/z/max_velocity", limiter_ang_.max_velocity, limiter_ang_.max_velocity);
|
||||
controller_nh.param(ns_ + "angular/z/min_velocity", limiter_ang_.min_velocity, -limiter_ang_.max_velocity);
|
||||
controller_nh.param(ns_ + "angular/z/max_acceleration", limiter_ang_.max_acceleration, limiter_ang_.max_acceleration);
|
||||
controller_nh.param(ns_ + "angular/z/min_acceleration", limiter_ang_.min_acceleration, -limiter_ang_.max_acceleration);
|
||||
controller_nh.param(ns_ + "angular/z/max_jerk", limiter_ang_.max_jerk, limiter_ang_.max_jerk);
|
||||
controller_nh.param(ns_ + "angular/z/min_jerk", limiter_ang_.min_jerk, -limiter_ang_.max_jerk);
|
||||
|
||||
// If either parameter is not available, we need to look up the value in the URDF
|
||||
bool lookup_wheel_separation_h = !controller_nh.getParam(ns_ + "wheel_separation_h", wheel_separation_h_);
|
||||
bool lookup_wheel_radius = !controller_nh.getParam(ns_ + "wheel_radius", wheel_radius_);
|
||||
|
||||
if (!setOdomParamsFromUrdf(root_nh,
|
||||
rear_wheel_name,
|
||||
front_steer_name,
|
||||
lookup_wheel_separation_h,
|
||||
lookup_wheel_radius))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// Regardless of how we got the separation and radius, use them
|
||||
// to set the odometry parameters
|
||||
const double ws_h = wheel_separation_h_multiplier_ * wheel_separation_h_;
|
||||
const double wr = wheel_radius_multiplier_ * wheel_radius_;
|
||||
odometry_.setWheelParams(ws_h, wr);
|
||||
ROS_INFO_STREAM_NAMED(name_,
|
||||
"Odometry params : wheel separation height " << ws_h
|
||||
<< ", wheel radius " << wr);
|
||||
|
||||
setOdomPubFields(root_nh, controller_nh);
|
||||
|
||||
//-- rear wheel
|
||||
//---- handles need to be previously registerd in steer_drive_test.cpp
|
||||
ROS_INFO_STREAM_NAMED(name_,
|
||||
"Adding the rear wheel with joint name: " << rear_wheel_name);
|
||||
rear_wheel_joint_ = vel_joint_if->getHandle(rear_wheel_name); // throws on failure
|
||||
//-- front steer
|
||||
ROS_INFO_STREAM_NAMED(name_,
|
||||
"Adding the front steer with joint name: " << front_steer_name);
|
||||
front_steer_joint_ = pos_joint_if->getHandle(front_steer_name); // throws on failure
|
||||
ROS_INFO_STREAM_NAMED(name_,
|
||||
"Adding the subscriber: cmd_vel");
|
||||
sub_command_ = controller_nh.subscribe("cmd_vel", 1, &SteerDriveController::cmdVelCallback, this);
|
||||
ROS_INFO_STREAM_NAMED(name_, "Finished controller initialization");
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void SteerDriveController::update(const ros::Time &time, const ros::Duration &period)
|
||||
{
|
||||
// COMPUTE AND PUBLISH ODOMETRY
|
||||
if (open_loop_)
|
||||
{
|
||||
odometry_.updateOpenLoop(last0_cmd_.lin, last0_cmd_.ang, time);
|
||||
}
|
||||
else
|
||||
{
|
||||
double wheel_pos = rear_wheel_joint_.getPosition();
|
||||
double steer_pos = front_steer_joint_.getPosition();
|
||||
|
||||
if (std::isnan(wheel_pos) || std::isnan(steer_pos))
|
||||
return;
|
||||
|
||||
// Estimate linear and angular velocity using joint information
|
||||
steer_pos = steer_pos * steer_pos_multiplier_;
|
||||
odometry_.update(wheel_pos, steer_pos, time);
|
||||
}
|
||||
|
||||
// Publish odometry message
|
||||
if (last_state_publish_time_ + publish_period_ < time)
|
||||
{
|
||||
last_state_publish_time_ += publish_period_;
|
||||
// Compute and store orientation info
|
||||
const geometry_msgs::Quaternion orientation(
|
||||
tf::createQuaternionMsgFromYaw(odometry_.getHeading()));
|
||||
|
||||
// Populate odom message and publish
|
||||
if (odom_pub_->trylock())
|
||||
{
|
||||
odom_pub_->msg_.header.stamp = time;
|
||||
odom_pub_->msg_.pose.pose.position.x = odometry_.getX();
|
||||
odom_pub_->msg_.pose.pose.position.y = odometry_.getY();
|
||||
odom_pub_->msg_.pose.pose.orientation = orientation;
|
||||
odom_pub_->msg_.twist.twist.linear.x = odometry_.getLinear();
|
||||
odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular();
|
||||
odom_pub_->unlockAndPublish();
|
||||
}
|
||||
|
||||
// Publish tf /odom frame
|
||||
if (enable_odom_tf_ && tf_odom_pub_->trylock())
|
||||
{
|
||||
geometry_msgs::TransformStamped &odom_frame = tf_odom_pub_->msg_.transforms[0];
|
||||
odom_frame.header.stamp = time;
|
||||
odom_frame.transform.translation.x = odometry_.getX();
|
||||
odom_frame.transform.translation.y = odometry_.getY();
|
||||
odom_frame.transform.rotation = orientation;
|
||||
tf_odom_pub_->unlockAndPublish();
|
||||
}
|
||||
}
|
||||
|
||||
// MOVE ROBOT
|
||||
// Retreive current velocity command and time step:
|
||||
Commands curr_cmd = *(command_.readFromRT());
|
||||
const double dt = (time - curr_cmd.stamp).toSec();
|
||||
|
||||
// Brake if cmd_vel has timeout:
|
||||
if (dt > cmd_vel_timeout_)
|
||||
{
|
||||
curr_cmd.lin = 0.0;
|
||||
curr_cmd.ang = 0.0;
|
||||
}
|
||||
|
||||
// Limit velocities and accelerations:
|
||||
const double cmd_dt(period.toSec());
|
||||
// double old_lin = curr_cmd.lin;
|
||||
limiter_lin_.limit(curr_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, cmd_dt);
|
||||
// double old_ang = curr_cmd.ang;
|
||||
limiter_ang_.limit(curr_cmd.ang, last0_cmd_.ang, last1_cmd_.ang, cmd_dt);
|
||||
|
||||
last1_cmd_ = last0_cmd_;
|
||||
last0_cmd_ = curr_cmd;
|
||||
|
||||
// Set Command
|
||||
const double wheel_vel = curr_cmd.lin / wheel_radius_; // omega = linear_vel / radius
|
||||
rear_wheel_joint_.setCommand(wheel_vel);
|
||||
front_steer_joint_.setCommand(curr_cmd.ang);
|
||||
// ROS_INFO("curr_cmd.lin = %f last0_cmd_.lin = %f last1_cmd_.lin = %f cmd_dt = %f", curr_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, old_lin);
|
||||
// ROS_WARN("curr_cmd.ang = %f last0_cmd_.ang = %f last1_cmd_.ang = %f cmd_dt = %f", curr_cmd.ang, last0_cmd_.ang, last1_cmd_.ang,old_ang);
|
||||
}
|
||||
|
||||
void SteerDriveController::starting(const ros::Time &time)
|
||||
{
|
||||
brake();
|
||||
|
||||
// Register starting time used to keep fixed rate
|
||||
last_state_publish_time_ = time;
|
||||
|
||||
odometry_.init(time);
|
||||
}
|
||||
|
||||
void SteerDriveController::stopping(const ros::Time & /*time*/)
|
||||
{
|
||||
brake();
|
||||
}
|
||||
|
||||
void SteerDriveController::brake()
|
||||
{
|
||||
const double steer_pos = 0.0;
|
||||
const double wheel_vel = 0.0;
|
||||
|
||||
rear_wheel_joint_.setCommand(steer_pos);
|
||||
front_steer_joint_.setCommand(wheel_vel);
|
||||
}
|
||||
|
||||
void SteerDriveController::cmdVelCallback(const geometry_msgs::Twist &command)
|
||||
{
|
||||
if (isRunning())
|
||||
{
|
||||
// check that we don't have multiple publishers on the command topic
|
||||
if (!allow_multiple_cmd_vel_publishers_ && sub_command_.getNumPublishers() > 1)
|
||||
{
|
||||
ROS_ERROR_STREAM_THROTTLE_NAMED(1.0, name_, "Detected " << sub_command_.getNumPublishers() << " publishers. Only 1 publisher is allowed. Going to brake.");
|
||||
brake();
|
||||
return;
|
||||
}
|
||||
|
||||
command_struct_.ang = command.angular.z;
|
||||
command_struct_.lin = command.linear.x;
|
||||
command_struct_.stamp = ros::Time::now();
|
||||
command_.writeFromNonRT(command_struct_);
|
||||
ROS_DEBUG_STREAM_NAMED(name_,
|
||||
"Added values to command. "
|
||||
<< "Ang: " << command_struct_.ang << ", "
|
||||
<< "Lin: " << command_struct_.lin << ", "
|
||||
<< "Stamp: " << command_struct_.stamp);
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running.");
|
||||
}
|
||||
}
|
||||
|
||||
bool SteerDriveController::setOdomParamsFromUrdf(ros::NodeHandle &root_nh,
|
||||
const std::string rear_wheel_name,
|
||||
const std::string front_steer_name,
|
||||
bool lookup_wheel_separation_h,
|
||||
bool lookup_wheel_radius)
|
||||
{
|
||||
if (!(lookup_wheel_separation_h || lookup_wheel_radius))
|
||||
{
|
||||
// Short-circuit in case we don't need to look up anything, so we don't have to parse the URDF
|
||||
return true;
|
||||
}
|
||||
|
||||
// Parse robot description
|
||||
const std::string model_param_name = "robot_description";
|
||||
bool res = root_nh.hasParam(model_param_name);
|
||||
std::string robot_model_str = "";
|
||||
if (!res || !root_nh.getParam(model_param_name, robot_model_str))
|
||||
{
|
||||
ROS_ERROR_NAMED(name_, "Robot descripion couldn't be retrieved from param server.");
|
||||
return false;
|
||||
}
|
||||
|
||||
urdf::ModelInterfaceSharedPtr model = urdf::parseURDF(robot_model_str);
|
||||
|
||||
urdf::JointConstSharedPtr rear_wheel_joint = model->getJoint(rear_wheel_name);
|
||||
urdf::JointConstSharedPtr front_steer_joint = model->getJoint(front_steer_name);
|
||||
|
||||
if (lookup_wheel_separation_h)
|
||||
{
|
||||
// Get wheel separation
|
||||
if (!rear_wheel_joint)
|
||||
{
|
||||
ROS_ERROR_STREAM_NAMED(name_, rear_wheel_name
|
||||
<< " couldn't be retrieved from model description");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!front_steer_joint)
|
||||
{
|
||||
ROS_ERROR_STREAM_NAMED(name_, front_steer_name
|
||||
<< " couldn't be retrieved from model description");
|
||||
return false;
|
||||
}
|
||||
|
||||
ROS_INFO_STREAM("rear wheel to origin: "
|
||||
<< rear_wheel_joint->parent_to_joint_origin_transform.position.x << ","
|
||||
<< rear_wheel_joint->parent_to_joint_origin_transform.position.y << ", "
|
||||
<< rear_wheel_joint->parent_to_joint_origin_transform.position.z);
|
||||
|
||||
ROS_INFO_STREAM("front steer to origin: "
|
||||
<< front_steer_joint->parent_to_joint_origin_transform.position.x << ","
|
||||
<< front_steer_joint->parent_to_joint_origin_transform.position.y << ", "
|
||||
<< front_steer_joint->parent_to_joint_origin_transform.position.z);
|
||||
|
||||
wheel_separation_h_ = fabs(
|
||||
rear_wheel_joint->parent_to_joint_origin_transform.position.x - front_steer_joint->parent_to_joint_origin_transform.position.x);
|
||||
|
||||
ROS_INFO_STREAM("Calculated wheel_separation_h: " << wheel_separation_h_);
|
||||
}
|
||||
|
||||
if (lookup_wheel_radius)
|
||||
{
|
||||
// Get wheel radius
|
||||
if (!getWheelRadius(model->getLink(rear_wheel_joint->child_link_name), wheel_radius_))
|
||||
{
|
||||
ROS_ERROR_STREAM_NAMED(name_, "Couldn't retrieve " << rear_wheel_name << " wheel radius");
|
||||
return false;
|
||||
}
|
||||
ROS_INFO_STREAM("Retrieved wheel_radius: " << wheel_radius_);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void SteerDriveController::setOdomPubFields(ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
|
||||
{
|
||||
// Get and check params for covariances
|
||||
XmlRpc::XmlRpcValue pose_cov_list;
|
||||
controller_nh.getParam(ns_ + "pose_covariance_diagonal", pose_cov_list);
|
||||
ROS_ASSERT(pose_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
|
||||
ROS_ASSERT(pose_cov_list.size() == 6);
|
||||
for (int i = 0; i < pose_cov_list.size(); ++i)
|
||||
ROS_ASSERT(pose_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
|
||||
|
||||
XmlRpc::XmlRpcValue twist_cov_list;
|
||||
controller_nh.getParam(ns_ + "twist_covariance_diagonal", twist_cov_list);
|
||||
ROS_ASSERT(twist_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
|
||||
ROS_ASSERT(twist_cov_list.size() == 6);
|
||||
for (int i = 0; i < twist_cov_list.size(); ++i)
|
||||
ROS_ASSERT(twist_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
|
||||
|
||||
// Setup odometry realtime publisher + odom message constant fields
|
||||
odom_pub_.reset(new realtime_tools::RealtimePublisher<nav_msgs::Odometry>(controller_nh, ns_ + "odom", 100));
|
||||
odom_pub_->msg_.header.frame_id = odom_frame_id_;
|
||||
odom_pub_->msg_.child_frame_id = base_frame_id_;
|
||||
odom_pub_->msg_.pose.pose.position.z = 0;
|
||||
odom_pub_->msg_.pose.covariance = boost::assign::list_of(static_cast<double>(pose_cov_list[0]))(0)(0)(0)(0)(0)(0)(static_cast<double>(pose_cov_list[1]))(0)(0)(0)(0)(0)(0)(static_cast<double>(pose_cov_list[2]))(0)(0)(0)(0)(0)(0)(static_cast<double>(pose_cov_list[3]))(0)(0)(0)(0)(0)(0)(static_cast<double>(pose_cov_list[4]))(0)(0)(0)(0)(0)(0)(static_cast<double>(pose_cov_list[5]));
|
||||
odom_pub_->msg_.twist.twist.linear.y = 0;
|
||||
odom_pub_->msg_.twist.twist.linear.z = 0;
|
||||
odom_pub_->msg_.twist.twist.angular.x = 0;
|
||||
odom_pub_->msg_.twist.twist.angular.y = 0;
|
||||
odom_pub_->msg_.twist.covariance = boost::assign::list_of(static_cast<double>(twist_cov_list[0]))(0)(0)(0)(0)(0)(0)(static_cast<double>(twist_cov_list[1]))(0)(0)(0)(0)(0)(0)(static_cast<double>(twist_cov_list[2]))(0)(0)(0)(0)(0)(0)(static_cast<double>(twist_cov_list[3]))(0)(0)(0)(0)(0)(0)(static_cast<double>(twist_cov_list[4]))(0)(0)(0)(0)(0)(0)(static_cast<double>(twist_cov_list[5]));
|
||||
tf_odom_pub_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(root_nh, "/tf", 100));
|
||||
tf_odom_pub_->msg_.transforms.resize(1);
|
||||
tf_odom_pub_->msg_.transforms[0].transform.translation.z = 0.0;
|
||||
tf_odom_pub_->msg_.transforms[0].child_frame_id = base_frame_id_;
|
||||
tf_odom_pub_->msg_.transforms[0].header.frame_id = odom_frame_id_;
|
||||
}
|
||||
|
||||
} // namespace steer_drive_controller
|
||||
@@ -0,0 +1,9 @@
|
||||
<library path="lib/libsteer_drive_controller">
|
||||
|
||||
<class name="steer_drive_controller/SteerDriveController" type="steer_drive_controller::SteerDriveController" base_class_type="controller_interface::ControllerBase">
|
||||
<description>
|
||||
The SteerDriveController tracks velocity commands. It expects 1 VelocityJointInterface and 1 PositionJointInterfece types of hardware interfaces.
|
||||
</description>
|
||||
</class>
|
||||
|
||||
</library>
|
||||
@@ -0,0 +1,13 @@
|
||||
steerbot_controller:
|
||||
type: "steer_drive_controller/SteerDriveController"
|
||||
rear_wheel : 'rear_wheel_joint'
|
||||
front_steer : 'front_steer_joint'
|
||||
publish_rate: 50.0 # defaults to 50
|
||||
|
||||
pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
|
||||
twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
|
||||
cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests
|
||||
|
||||
#wheel_separation_h : 0.4
|
||||
#wheel_radius : 0.11
|
||||
wheel_separation_h_multiplier: 0.257 # calibration parameter for odometory, needed for test.
|
||||
@@ -0,0 +1,9 @@
|
||||
steerbot_hw_sim:
|
||||
rear_wheel : 'rear_wheel_joint'
|
||||
front_steer : 'front_steer_joint'
|
||||
|
||||
rear_wheels: ['rear_right_wheel_joint', 'rear_left_wheel_joint']
|
||||
front_wheels: ['front_right_wheel_joint', 'front_left_wheel_joint']
|
||||
front_steers: ['front_right_steer_joint', 'front_left_steer_joint']
|
||||
|
||||
wheel_separation_h : 0.20
|
||||
@@ -0,0 +1,434 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// Copyright (C) 2013, PAL Robotics S.L.
|
||||
//
|
||||
// Redistribution and use in source and binary forms, with or without
|
||||
// modification, are permitted provided that the following conditions are met:
|
||||
// * Redistributions of source code must retain the above copyright notice,
|
||||
// this list of conditions and the following disclaimer.
|
||||
// * Redistributions in binary form must reproduce the above copyright
|
||||
// notice, this list of conditions and the following disclaimer in the
|
||||
// documentation and/or other materials provided with the distribution.
|
||||
// * Neither the name of PAL Robotics, Inc. nor the names of its
|
||||
// contributors may be used to endorse or promote products derived from
|
||||
// this software without specific prior written permission.
|
||||
//
|
||||
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
// POSSIBILITY OF SUCH DAMAGE.
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
// NOTE: The contents of this file have been taken largely from the ros_control wiki tutorials
|
||||
|
||||
// ROS
|
||||
#include <ros/ros.h>
|
||||
#include <std_msgs/Float64.h>
|
||||
#include <std_srvs/Empty.h>
|
||||
|
||||
// ros_control
|
||||
#include <controller_manager/controller_manager.h>
|
||||
#include <hardware_interface/joint_command_interface.h>
|
||||
#include <hardware_interface/joint_state_interface.h>
|
||||
#include <hardware_interface/robot_hw.h>
|
||||
#include <realtime_tools/realtime_buffer.h>
|
||||
|
||||
// NaN
|
||||
#include <limits>
|
||||
|
||||
// ostringstream
|
||||
#include <sstream>
|
||||
|
||||
enum INDEX_WHEEL {
|
||||
INDEX_RIGHT = 0,
|
||||
INDEX_LEFT = 1
|
||||
};
|
||||
|
||||
class Steerbot : public hardware_interface::RobotHW
|
||||
{
|
||||
public:
|
||||
Steerbot()
|
||||
: running_(true)
|
||||
, start_srv_(nh_.advertiseService("start", &Steerbot::startCallback, this))
|
||||
, stop_srv_(nh_.advertiseService("stop", &Steerbot::stopCallback, this))
|
||||
, ns_("steerbot_hw_sim/")
|
||||
{
|
||||
// Intialize raw data
|
||||
this->cleanUp();
|
||||
this->getJointNames(nh_);
|
||||
this->registerHardwareInterfaces();
|
||||
|
||||
nh_.getParam(ns_ + "wheel_separation_w", wheel_separation_w_);
|
||||
nh_.getParam(ns_ + "wheel_separation_h", wheel_separation_h_);
|
||||
ROS_INFO_STREAM("wheel_separation_w in test steerbot= " << wheel_separation_w_);
|
||||
ROS_INFO_STREAM("wheel_separation_h in test steerbot= " << wheel_separation_h_);
|
||||
}
|
||||
|
||||
ros::Time getTime() const {return ros::Time::now();}
|
||||
ros::Duration getPeriod() const {return ros::Duration(0.01);}
|
||||
|
||||
void read()
|
||||
{
|
||||
std::ostringstream os;
|
||||
// directly get from controller
|
||||
os << rear_wheel_jnt_vel_cmd_ << ", ";
|
||||
os << front_steer_jnt_pos_cmd_ << ", ";
|
||||
|
||||
// convert to each joint velocity
|
||||
//-- differential drive
|
||||
for (unsigned int i = 0; i < virtual_rear_wheel_jnt_vel_cmd_.size(); i++)
|
||||
{
|
||||
virtual_rear_wheel_jnt_vel_cmd_[i] = rear_wheel_jnt_vel_cmd_;
|
||||
os << virtual_rear_wheel_jnt_vel_cmd_[i] << ", ";
|
||||
}
|
||||
|
||||
//-- ackerman link
|
||||
const double h = wheel_separation_h_;
|
||||
const double w = wheel_separation_w_;
|
||||
virtual_front_steer_jnt_pos_cmd_[INDEX_RIGHT] = atan2(2*h*tan(front_steer_jnt_pos_cmd_), 2*h + w/2.0*tan(front_steer_jnt_pos_cmd_));
|
||||
virtual_front_steer_jnt_pos_cmd_[INDEX_LEFT] = atan2(2*h*tan(front_steer_jnt_pos_cmd_), 2*h - w/2.0*tan(front_steer_jnt_pos_cmd_));
|
||||
|
||||
for (unsigned int i = 0; i < virtual_front_steer_jnt_pos_cmd_.size(); i++)
|
||||
{
|
||||
os << virtual_front_steer_jnt_pos_cmd_[i] << ", ";
|
||||
}
|
||||
|
||||
if (rear_wheel_jnt_vel_cmd_ != 0.0 || front_steer_jnt_pos_cmd_ != 0.0)
|
||||
ROS_INFO_STREAM("Commands for joints: " << os.str());
|
||||
|
||||
}
|
||||
|
||||
void write()
|
||||
{
|
||||
std::ostringstream os;
|
||||
if (running_)
|
||||
{
|
||||
// wheels
|
||||
rear_wheel_jnt_pos_ += rear_wheel_jnt_vel_*getPeriod().toSec();
|
||||
rear_wheel_jnt_vel_ = rear_wheel_jnt_vel_cmd_;
|
||||
for (unsigned int i = 0; i < virtual_rear_wheel_jnt_vel_cmd_.size(); i++)
|
||||
{
|
||||
// Note that pos_[i] will be NaN for one more cycle after we start(),
|
||||
// but that is consistent with the knowledge we have about the state
|
||||
// of the robot.
|
||||
virtual_rear_wheel_jnt_pos_[i] += virtual_rear_wheel_jnt_vel_[i]*getPeriod().toSec();
|
||||
virtual_rear_wheel_jnt_vel_[i] = virtual_rear_wheel_jnt_vel_cmd_[i];
|
||||
}
|
||||
|
||||
// steers
|
||||
front_steer_jnt_pos_ = front_steer_jnt_pos_cmd_;
|
||||
for (unsigned int i = 0; i < virtual_front_steer_jnt_pos_cmd_.size(); i++)
|
||||
{
|
||||
virtual_front_steer_jnt_pos_[i] = virtual_front_steer_jnt_pos_cmd_[i];
|
||||
}
|
||||
|
||||
// directly get from controller
|
||||
os << rear_wheel_jnt_vel_cmd_ << ", ";
|
||||
os << front_steer_jnt_pos_cmd_ << ", ";
|
||||
|
||||
// convert to each joint velocity
|
||||
//-- differential drive
|
||||
for (unsigned int i = 0; i < virtual_rear_wheel_jnt_pos_.size(); i++)
|
||||
{
|
||||
os << virtual_rear_wheel_jnt_pos_[i] << ", ";
|
||||
}
|
||||
|
||||
//-- ackerman link
|
||||
for (unsigned int i = 0; i < virtual_front_steer_jnt_pos_.size(); i++)
|
||||
{
|
||||
os << virtual_front_steer_jnt_pos_[i] << ", ";
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// wheels
|
||||
rear_wheel_jnt_pos_= std::numeric_limits<double>::quiet_NaN();
|
||||
rear_wheel_jnt_vel_= std::numeric_limits<double>::quiet_NaN();
|
||||
std::fill_n(virtual_rear_wheel_jnt_pos_.begin(), virtual_rear_wheel_jnt_pos_.size(), std::numeric_limits<double>::quiet_NaN());
|
||||
std::fill_n(virtual_rear_wheel_jnt_vel_.begin(), virtual_rear_wheel_jnt_vel_.size(), std::numeric_limits<double>::quiet_NaN());
|
||||
// steers
|
||||
front_steer_jnt_pos_= std::numeric_limits<double>::quiet_NaN();
|
||||
front_steer_jnt_vel_= std::numeric_limits<double>::quiet_NaN();
|
||||
std::fill_n(virtual_front_steer_jnt_pos_.begin(), virtual_front_steer_jnt_pos_.size(), std::numeric_limits<double>::quiet_NaN());
|
||||
std::fill_n(virtual_front_steer_jnt_vel_.begin(), virtual_front_steer_jnt_vel_.size(), std::numeric_limits<double>::quiet_NaN());
|
||||
|
||||
// wheels
|
||||
os << rear_wheel_jnt_pos_ << ", ";
|
||||
os << rear_wheel_jnt_vel_ << ", ";
|
||||
for (unsigned int i = 0; i < virtual_rear_wheel_jnt_pos_.size(); i++)
|
||||
{
|
||||
os << virtual_rear_wheel_jnt_pos_[i] << ", ";
|
||||
}
|
||||
|
||||
// steers
|
||||
os << front_steer_jnt_pos_ << ", ";
|
||||
os << front_steer_jnt_vel_ << ", ";
|
||||
for (unsigned int i = 0; i < virtual_front_steer_jnt_pos_.size(); i++)
|
||||
{
|
||||
os << virtual_front_steer_jnt_pos_[i] << ", ";
|
||||
}
|
||||
}
|
||||
ROS_INFO_STREAM("running_ = " << running_ << ". commands are " << os.str());
|
||||
}
|
||||
|
||||
bool startCallback(std_srvs::Empty::Request& /*req*/, std_srvs::Empty::Response& /*res*/)
|
||||
{
|
||||
ROS_INFO_STREAM("running_ = " << running_ << ".");
|
||||
running_ = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool stopCallback(std_srvs::Empty::Request& /*req*/, std_srvs::Empty::Response& /*res*/)
|
||||
{
|
||||
ROS_INFO_STREAM("running_ = " << running_ << ".");
|
||||
running_ = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
void cleanUp()
|
||||
{
|
||||
// wheel
|
||||
//-- wheel joint names
|
||||
rear_wheel_jnt_name_.empty();
|
||||
virtual_rear_wheel_jnt_names_.clear();
|
||||
//-- actual rear wheel joint
|
||||
rear_wheel_jnt_pos_ = 0;
|
||||
rear_wheel_jnt_vel_ = 0;
|
||||
rear_wheel_jnt_eff_ = 0;
|
||||
rear_wheel_jnt_vel_cmd_ = 0;
|
||||
//-- virtual rear wheel joint
|
||||
virtual_rear_wheel_jnt_pos_.clear();
|
||||
virtual_rear_wheel_jnt_vel_.clear();
|
||||
virtual_rear_wheel_jnt_eff_.clear();
|
||||
virtual_rear_wheel_jnt_vel_cmd_.clear();
|
||||
//-- virtual front wheel joint
|
||||
virtual_front_wheel_jnt_pos_.clear();
|
||||
virtual_front_wheel_jnt_vel_.clear();
|
||||
virtual_front_wheel_jnt_eff_.clear();
|
||||
virtual_front_wheel_jnt_vel_cmd_.clear();
|
||||
|
||||
// steer
|
||||
//-- steer joint names
|
||||
front_steer_jnt_name_.empty();
|
||||
virtual_front_steer_jnt_names_.clear();
|
||||
//-- front steer joint
|
||||
front_steer_jnt_pos_ = 0;
|
||||
front_steer_jnt_vel_ = 0;
|
||||
front_steer_jnt_eff_ = 0;
|
||||
front_steer_jnt_pos_cmd_ = 0;
|
||||
//-- virtual wheel joint
|
||||
virtual_front_steer_jnt_pos_.clear();
|
||||
virtual_front_steer_jnt_vel_.clear();
|
||||
virtual_front_steer_jnt_eff_.clear();
|
||||
virtual_front_steer_jnt_pos_cmd_.clear();
|
||||
}
|
||||
|
||||
void getJointNames(ros::NodeHandle &_nh)
|
||||
{
|
||||
this->getWheelJointNames(_nh);
|
||||
this->getSteerJointNames(_nh);
|
||||
}
|
||||
|
||||
void getWheelJointNames(ros::NodeHandle &_nh)
|
||||
{
|
||||
// wheel joint to get linear command
|
||||
_nh.getParam(ns_ + "rear_wheel", rear_wheel_jnt_name_);
|
||||
|
||||
// virtual wheel joint for gazebo con_rol
|
||||
_nh.getParam(ns_ + "rear_wheels", virtual_rear_wheel_jnt_names_);
|
||||
int dof = virtual_rear_wheel_jnt_names_.size();
|
||||
virtual_rear_wheel_jnt_pos_.resize(dof);
|
||||
virtual_rear_wheel_jnt_vel_.resize(dof);
|
||||
virtual_rear_wheel_jnt_eff_.resize(dof);
|
||||
virtual_rear_wheel_jnt_vel_cmd_.resize(dof);
|
||||
|
||||
_nh.getParam(ns_ + "front_wheels", virtual_front_wheel_jnt_names_);
|
||||
dof = virtual_front_wheel_jnt_names_.size();
|
||||
virtual_front_wheel_jnt_pos_.resize(dof);
|
||||
virtual_front_wheel_jnt_vel_.resize(dof);
|
||||
virtual_front_wheel_jnt_eff_.resize(dof);
|
||||
virtual_front_wheel_jnt_vel_cmd_.resize(dof);
|
||||
}
|
||||
|
||||
void getSteerJointNames(ros::NodeHandle &_nh)
|
||||
{
|
||||
// steer joint to get angular command
|
||||
_nh.getParam(ns_ + "front_steer", front_steer_jnt_name_);
|
||||
|
||||
// virtual steer joint for gazebo control
|
||||
_nh.getParam(ns_ + "front_steers", virtual_front_steer_jnt_names_);
|
||||
|
||||
const int dof = virtual_front_steer_jnt_names_.size();
|
||||
virtual_front_steer_jnt_pos_.resize(dof);
|
||||
virtual_front_steer_jnt_vel_.resize(dof);
|
||||
virtual_front_steer_jnt_eff_.resize(dof);
|
||||
virtual_front_steer_jnt_pos_cmd_.resize(dof);
|
||||
}
|
||||
|
||||
void registerHardwareInterfaces()
|
||||
{
|
||||
this->registerSteerInterface();
|
||||
this->registerWheelInterface();
|
||||
|
||||
// register mapped interface to ros_control
|
||||
registerInterface(&jnt_state_interface_);
|
||||
registerInterface(&rear_wheel_jnt_vel_cmd_interface_);
|
||||
registerInterface(&front_steer_jnt_pos_cmd_interface_);
|
||||
}
|
||||
|
||||
void registerWheelInterface()
|
||||
{
|
||||
// actual wheel joints
|
||||
this->registerInterfaceHandles(
|
||||
jnt_state_interface_, rear_wheel_jnt_vel_cmd_interface_,
|
||||
rear_wheel_jnt_name_, rear_wheel_jnt_pos_, rear_wheel_jnt_vel_, rear_wheel_jnt_eff_, rear_wheel_jnt_vel_cmd_);
|
||||
|
||||
// virtual rear wheel joints
|
||||
for (int i = 0; i < virtual_rear_wheel_jnt_names_.size(); i++)
|
||||
{
|
||||
this->registerInterfaceHandles(
|
||||
jnt_state_interface_, rear_wheel_jnt_vel_cmd_interface_,
|
||||
virtual_rear_wheel_jnt_names_[i], virtual_rear_wheel_jnt_pos_[i], virtual_rear_wheel_jnt_vel_[i], virtual_rear_wheel_jnt_eff_[i], virtual_rear_wheel_jnt_vel_cmd_[i]);
|
||||
}
|
||||
// virtual front wheel joints
|
||||
for (int i = 0; i < virtual_front_wheel_jnt_names_.size(); i++)
|
||||
{
|
||||
this->registerInterfaceHandles(
|
||||
jnt_state_interface_, rear_wheel_jnt_vel_cmd_interface_,
|
||||
virtual_front_wheel_jnt_names_[i], virtual_front_wheel_jnt_pos_[i], virtual_front_wheel_jnt_vel_[i], virtual_front_wheel_jnt_eff_[i], virtual_front_wheel_jnt_vel_cmd_[i]);
|
||||
}
|
||||
}
|
||||
|
||||
void registerSteerInterface()
|
||||
{
|
||||
// actual steer joints
|
||||
this->registerInterfaceHandles(
|
||||
jnt_state_interface_, front_steer_jnt_pos_cmd_interface_,
|
||||
front_steer_jnt_name_, front_steer_jnt_pos_, front_steer_jnt_vel_, front_steer_jnt_eff_, front_steer_jnt_pos_cmd_);
|
||||
|
||||
// virtual steer joints
|
||||
for (int i = 0; i < virtual_front_steer_jnt_names_.size(); i++)
|
||||
{
|
||||
this->registerInterfaceHandles(
|
||||
jnt_state_interface_, front_steer_jnt_pos_cmd_interface_,
|
||||
virtual_front_steer_jnt_names_[i], virtual_front_steer_jnt_pos_[i], virtual_front_steer_jnt_vel_[i], virtual_front_steer_jnt_eff_[i], virtual_front_steer_jnt_pos_cmd_[i]);
|
||||
}
|
||||
}
|
||||
|
||||
void registerInterfaceHandles(
|
||||
hardware_interface::JointStateInterface& _jnt_state_interface,
|
||||
hardware_interface::JointCommandInterface& _jnt_cmd_interface,
|
||||
const std::string _jnt_name, double& _jnt_pos, double& _jnt_vel, double& _jnt_eff, double& _jnt_cmd)
|
||||
{
|
||||
// register joint (both JointState and CommandJoint)
|
||||
this->registerJointStateInterfaceHandle(_jnt_state_interface, _jnt_name,
|
||||
_jnt_pos, _jnt_vel, _jnt_eff);
|
||||
this->registerCommandJointInterfaceHandle(_jnt_state_interface, _jnt_cmd_interface,
|
||||
_jnt_name, _jnt_cmd);
|
||||
}
|
||||
|
||||
void registerJointStateInterfaceHandle(
|
||||
hardware_interface::JointStateInterface& _jnt_state_interface,
|
||||
const std::string _jnt_name, double& _jnt_pos, double& _jnt_vel, double& _jnt_eff)
|
||||
{
|
||||
hardware_interface::JointStateHandle state_handle(_jnt_name,
|
||||
&_jnt_pos,
|
||||
&_jnt_vel,
|
||||
&_jnt_eff);
|
||||
_jnt_state_interface.registerHandle(state_handle);
|
||||
|
||||
ROS_INFO_STREAM("Registered joint '" << _jnt_name << " ' in the JointStateInterface");
|
||||
}
|
||||
|
||||
void registerCommandJointInterfaceHandle(
|
||||
hardware_interface::JointStateInterface& _jnt_state_interface,
|
||||
hardware_interface::JointCommandInterface& _jnt_cmd_interface,
|
||||
const std::string _jnt_name, double& _jnt_cmd)
|
||||
{
|
||||
// joint command
|
||||
hardware_interface::JointHandle _handle(_jnt_state_interface.getHandle(_jnt_name),
|
||||
&_jnt_cmd);
|
||||
_jnt_cmd_interface.registerHandle(_handle);
|
||||
|
||||
ROS_INFO_STREAM("Registered joint '" << _jnt_name << " ' in the CommandJointInterface");
|
||||
}
|
||||
|
||||
private:
|
||||
// common
|
||||
hardware_interface::JointStateInterface jnt_state_interface_;// rear wheel
|
||||
//-- actual joint(single actuator)
|
||||
//---- joint name
|
||||
std::string rear_wheel_jnt_name_;
|
||||
//---- joint interface parameters
|
||||
double rear_wheel_jnt_pos_;
|
||||
double rear_wheel_jnt_vel_;
|
||||
double rear_wheel_jnt_eff_;
|
||||
//---- joint interface command
|
||||
double rear_wheel_jnt_vel_cmd_;
|
||||
//---- Hardware interface: joint
|
||||
hardware_interface::VelocityJointInterface rear_wheel_jnt_vel_cmd_interface_;
|
||||
//hardware_interface::JointStateInterface wheel_jnt_state_interface_;
|
||||
//
|
||||
//-- virtual joints(two rear wheels)
|
||||
//---- joint name
|
||||
std::vector<std::string> virtual_rear_wheel_jnt_names_;
|
||||
//---- joint interface parameters
|
||||
std::vector<double> virtual_rear_wheel_jnt_pos_;
|
||||
std::vector<double> virtual_rear_wheel_jnt_vel_;
|
||||
std::vector<double> virtual_rear_wheel_jnt_eff_;
|
||||
//---- joint interface command
|
||||
std::vector<double> virtual_rear_wheel_jnt_vel_cmd_;
|
||||
//-- virtual joints(two front wheels)
|
||||
//---- joint name
|
||||
std::vector<std::string> virtual_front_wheel_jnt_names_;
|
||||
//---- joint interface parameters
|
||||
std::vector<double> virtual_front_wheel_jnt_pos_;
|
||||
std::vector<double> virtual_front_wheel_jnt_vel_;
|
||||
std::vector<double> virtual_front_wheel_jnt_eff_;
|
||||
//---- joint interface command
|
||||
std::vector<double> virtual_front_wheel_jnt_vel_cmd_;
|
||||
|
||||
// front steer
|
||||
//-- actual joint(single actuator)
|
||||
//---- joint name
|
||||
std::string front_steer_jnt_name_;
|
||||
//---- joint interface parameters
|
||||
double front_steer_jnt_pos_;
|
||||
double front_steer_jnt_vel_;
|
||||
double front_steer_jnt_eff_;
|
||||
//---- joint interface command
|
||||
double front_steer_jnt_pos_cmd_;
|
||||
//---- Hardware interface: joint
|
||||
hardware_interface::PositionJointInterface front_steer_jnt_pos_cmd_interface_;
|
||||
//hardware_interface::JointStateInterface steer_jnt_state_interface_;
|
||||
//
|
||||
//-- virtual joints(two steers)
|
||||
//---- joint name
|
||||
std::vector<std::string> virtual_front_steer_jnt_names_;
|
||||
//---- joint interface parameters
|
||||
std::vector<double> virtual_front_steer_jnt_pos_;
|
||||
std::vector<double> virtual_front_steer_jnt_vel_;
|
||||
std::vector<double> virtual_front_steer_jnt_eff_;
|
||||
//---- joint interface command
|
||||
std::vector<double> virtual_front_steer_jnt_pos_cmd_;
|
||||
|
||||
// Wheel separation, wrt the midpoint of the wheel width:
|
||||
double wheel_separation_w_;
|
||||
// Wheel separation, wrt the midpoint of the wheel width:
|
||||
double wheel_separation_h_;
|
||||
|
||||
std::string ns_;
|
||||
bool running_;
|
||||
|
||||
ros::NodeHandle nh_;
|
||||
ros::ServiceServer start_srv_;
|
||||
ros::ServiceServer stop_srv_;
|
||||
};
|
||||
@@ -0,0 +1,97 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// Copyright (C) 2013, PAL Robotics S.L.
|
||||
//
|
||||
// Redistribution and use in source and binary forms, with or without
|
||||
// modification, are permitted provided that the following conditions are met:
|
||||
// * Redistributions of source code must retain the above copyright notice,
|
||||
// this list of conditions and the following disclaimer.
|
||||
// * Redistributions in binary form must reproduce the above copyright
|
||||
// notice, this list of conditions and the following disclaimer in the
|
||||
// documentation and/or other materials provided with the distribution.
|
||||
// * Neither the name of PAL Robotics, Inc. nor the names of its
|
||||
// contributors may be used to endorse or promote products derived from
|
||||
// this software without specific prior written permission.
|
||||
//
|
||||
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
// POSSIBILITY OF SUCH DAMAGE.
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/// \author Bence Magyar
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
|
||||
#include <geometry_msgs/Twist.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <tf/tf.h>
|
||||
|
||||
#include <std_srvs/Empty.h>
|
||||
|
||||
// Floating-point value comparison threshold
|
||||
const double EPS = 0.01;
|
||||
const double POSITION_TOLERANCE = 0.01; // 1 cm-s precision
|
||||
const double VELOCITY_TOLERANCE = 0.02; // 2 cm-s-1 precision
|
||||
const double ANGULAR_VELOCITY_TOLERANCE = 0.05; // 3 deg-s-1 precision
|
||||
const double JERK_LINEAR_VELOCITY_TOLERANCE = 0.10; // 10 cm-s-1 precision
|
||||
const double JERK_ANGULAR_VELOCITY_TOLERANCE = 0.05; // 3 deg-s-1 precision
|
||||
const double ORIENTATION_TOLERANCE = 0.03; // 0.57 degree precision
|
||||
|
||||
class SteerDriveControllerTest : public ::testing::Test
|
||||
{
|
||||
public:
|
||||
|
||||
SteerDriveControllerTest()
|
||||
: cmd_pub(nh.advertise<geometry_msgs::Twist>("cmd_vel", 100))
|
||||
, odom_sub(nh.subscribe("odom", 100, &SteerDriveControllerTest::odomCallback, this))
|
||||
, start_srv(nh.serviceClient<std_srvs::Empty>("start"))
|
||||
, stop_srv(nh.serviceClient<std_srvs::Empty>("stop"))
|
||||
{
|
||||
}
|
||||
|
||||
~SteerDriveControllerTest()
|
||||
{
|
||||
odom_sub.shutdown();
|
||||
}
|
||||
|
||||
nav_msgs::Odometry getLastOdom(){ return last_odom; }
|
||||
void publish(geometry_msgs::Twist cmd_vel){ cmd_pub.publish(cmd_vel); }
|
||||
bool isControllerAlive(){ return (odom_sub.getNumPublishers() > 0) && (cmd_pub.getNumSubscribers() > 0); }
|
||||
|
||||
void start(){ std_srvs::Empty srv; start_srv.call(srv); }
|
||||
void stop(){ std_srvs::Empty srv; stop_srv.call(srv); }
|
||||
|
||||
private:
|
||||
ros::NodeHandle nh;
|
||||
ros::Publisher cmd_pub;
|
||||
ros::Subscriber odom_sub;
|
||||
nav_msgs::Odometry last_odom;
|
||||
|
||||
ros::ServiceClient start_srv;
|
||||
ros::ServiceClient stop_srv;
|
||||
|
||||
void odomCallback(const nav_msgs::Odometry& odom)
|
||||
{
|
||||
ROS_INFO_STREAM("Callback reveived: pos.x: " << odom.pose.pose.position.x
|
||||
<< ", orient.z: " << odom.pose.pose.orientation.z
|
||||
<< ", lin_est: " << odom.twist.twist.linear.x
|
||||
<< ", ang_est: " << odom.twist.twist.angular.z);
|
||||
last_odom = odom;
|
||||
}
|
||||
};
|
||||
|
||||
inline tf::Quaternion tfQuatFromGeomQuat(const geometry_msgs::Quaternion& quat)
|
||||
{
|
||||
return tf::Quaternion(quat.x, quat.y, quat.z, quat.w);
|
||||
}
|
||||
@@ -0,0 +1,37 @@
|
||||
<launch>
|
||||
<!-- Load steerbot model -->
|
||||
<param name="robot_description"
|
||||
command="$(find xacro)/xacro '$(find steer_drive_controller)/test/common/urdf/steerbot.xacro' --inorder" />
|
||||
|
||||
<!-- Load controller config -->
|
||||
<rosparam command="load" file="$(find steer_drive_controller)/test/common/config/steerbot_controllers.yaml" />
|
||||
|
||||
<!-- Load steerbot config -->
|
||||
<rosparam command="load" file="$(find steer_drive_controller)/test/common/config/steerbot_hw_sim.yaml" />
|
||||
|
||||
<!-- Start steerbot -->
|
||||
<node name="steer_drive_controller_steerbot"
|
||||
pkg="steer_drive_controller"
|
||||
type="steer_drive_controller_steerbot"/>
|
||||
|
||||
<!-- Spawn controller -->
|
||||
<node name="controller_spawner"
|
||||
pkg="controller_manager" type="spawner" output="screen"
|
||||
args="steerbot_controller" />
|
||||
|
||||
<!-- rqt_plot monitoring -->
|
||||
<!--
|
||||
<node name="steerbot_pos_monitor"
|
||||
pkg="rqt_plot"
|
||||
type="rqt_plot"
|
||||
args="/steerbot_controller/odom/pose/pose/position/x" />
|
||||
<node name="steerbot_vel_monitor"
|
||||
pkg="rqt_plot"
|
||||
type="rqt_plot"
|
||||
args="/steerbot_controller/odom/twist/twist/linear/x" />
|
||||
<node name="steerbot_ang_vel_monitor"
|
||||
pkg="rqt_plot"
|
||||
type="rqt_plot"
|
||||
args="/steerbot_controller/odom/twist/twist/angular/z" />
|
||||
-->
|
||||
</launch>
|
||||
@@ -0,0 +1,32 @@
|
||||
<launch>
|
||||
<!-- start world -->
|
||||
<include file="$(find gazebo_ros)/launch/empty_world.launch"/>
|
||||
|
||||
<!-- load robot -->
|
||||
<param name="robot_description"
|
||||
command="$(find xacro)/xacro '$(find steer_drive_controller)/test/common/urdf/steerbot.xacro' --inorder" />
|
||||
|
||||
<!-- Load controller config -->
|
||||
<rosparam command="load" file="$(find steer_drive_controller)/test/common/config/steerbot_controllers.yaml" />
|
||||
|
||||
<!-- Load steerbot config -->
|
||||
<rosparam command="load" file="$(find steer_drive_controller)/test/common/config/steerbot_hw_sim.yaml" />
|
||||
|
||||
<!-- Start steerbot -->
|
||||
<node name="steer_drive_controller_steerbot"
|
||||
pkg="steer_drive_controller"
|
||||
type="steer_drive_controller_steerbot"/>
|
||||
|
||||
<!-- Spawn controller -->
|
||||
<node name="controller_spawner"
|
||||
pkg="controller_manager" type="spawner" output="screen"
|
||||
args="steerbot_controller" />
|
||||
|
||||
<!-- push robot_description to factory and spawn robot in gazebo -->
|
||||
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
|
||||
args="-urdf -model steerbot -param robot_description -z 0.5"/>
|
||||
|
||||
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
|
||||
<node name="rviz" pkg="rviz" type="rviz" required="true" args="-d $(find steer_drive_controller)/test/common/rviz/display.rviz"/>
|
||||
</launch>
|
||||
@@ -0,0 +1,256 @@
|
||||
Panels:
|
||||
- Class: rviz/Displays
|
||||
Help Height: 78
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
- /TF1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 775
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
Expanded:
|
||||
- /2D Pose Estimate1
|
||||
- /2D Nav Goal1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.588679
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: ""
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.03
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Class: rviz/RobotModel
|
||||
Collision Enabled: false
|
||||
Enabled: true
|
||||
Links:
|
||||
All Links Enabled: true
|
||||
Expand Joint Details: false
|
||||
Expand Link Details: false
|
||||
Expand Tree: false
|
||||
Link Tree Style: Links in Alphabetic Order
|
||||
base_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
camera_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
front_bottom_lrf:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
front_top_3durg:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
front_top_3durg_rgbd_optical_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
left_front_wheel:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
left_rear_wheel:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
merged_lrf:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
rear_bottom_lrf:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_front_wheel:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_rear_wheel:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
steer:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
steer_left:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
steer_right:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Name: RobotModel
|
||||
Robot Description: robot_description
|
||||
TF Prefix: ""
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Visual Enabled: true
|
||||
- Class: rviz/TF
|
||||
Enabled: true
|
||||
Frame Timeout: 15
|
||||
Frames:
|
||||
All Enabled: true
|
||||
base_link:
|
||||
Value: true
|
||||
camera_link:
|
||||
Value: true
|
||||
front_bottom_lrf:
|
||||
Value: true
|
||||
front_top_3durg:
|
||||
Value: true
|
||||
front_top_3durg_rgbd_optical_frame:
|
||||
Value: true
|
||||
left_front_wheel:
|
||||
Value: true
|
||||
left_rear_wheel:
|
||||
Value: true
|
||||
merged_lrf:
|
||||
Value: true
|
||||
rear_bottom_lrf:
|
||||
Value: true
|
||||
right_front_wheel:
|
||||
Value: true
|
||||
right_rear_wheel:
|
||||
Value: true
|
||||
steer:
|
||||
Value: true
|
||||
steer_left:
|
||||
Value: true
|
||||
steer_right:
|
||||
Value: true
|
||||
Marker Scale: 1
|
||||
Name: TF
|
||||
Show Arrows: true
|
||||
Show Axes: true
|
||||
Show Names: true
|
||||
Tree:
|
||||
base_link:
|
||||
camera_link:
|
||||
{}
|
||||
front_bottom_lrf:
|
||||
{}
|
||||
front_top_3durg:
|
||||
front_top_3durg_rgbd_optical_frame:
|
||||
{}
|
||||
left_rear_wheel:
|
||||
{}
|
||||
merged_lrf:
|
||||
{}
|
||||
rear_bottom_lrf:
|
||||
{}
|
||||
right_rear_wheel:
|
||||
{}
|
||||
steer:
|
||||
{}
|
||||
steer_left:
|
||||
left_front_wheel:
|
||||
{}
|
||||
steer_right:
|
||||
right_front_wheel:
|
||||
{}
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Fixed Frame: base_link
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz/MoveCamera
|
||||
- Class: rviz/Select
|
||||
- Class: rviz/FocusCamera
|
||||
- Class: rviz/Measure
|
||||
- Class: rviz/SetInitialPose
|
||||
Topic: /initialpose
|
||||
- Class: rviz/SetGoal
|
||||
Topic: /move_base_simple/goal
|
||||
- Class: rviz/PublishPoint
|
||||
Single click: true
|
||||
Topic: /clicked_point
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz/Orbit
|
||||
Distance: 2.57916
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.06
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: -0.129039
|
||||
Y: -0.142336
|
||||
Z: 0.339095
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.01
|
||||
Pitch: 0.420398
|
||||
Target Frame: base_link
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 0.805398
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 1056
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: true
|
||||
QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007510000003efc0100000002fb0000000800540069006d0065010000000000000751000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000005e10000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: true
|
||||
Width: 1873
|
||||
X: 47
|
||||
Y: 24
|
||||
@@ -0,0 +1,61 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// Copyright (C) 2013, PAL Robotics S.L.
|
||||
//
|
||||
// Redistribution and use in source and binary forms, with or without
|
||||
// modification, are permitted provided that the following conditions are met:
|
||||
// * Redistributions of source code must retain the above copyright notice,
|
||||
// this list of conditions and the following disclaimer.
|
||||
// * Redistributions in binary form must reproduce the above copyright
|
||||
// notice, this list of conditions and the following disclaimer in the
|
||||
// documentation and/or other materials provided with the distribution.
|
||||
// * Neither the name of PAL Robotics, Inc. nor the names of its
|
||||
// contributors may be used to endorse or promote products derived from
|
||||
// this software without specific prior written permission.
|
||||
//
|
||||
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
// POSSIBILITY OF SUCH DAMAGE.
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
// NOTE: The contents of this file have been taken largely from the ros_control wiki tutorials
|
||||
|
||||
// ROS
|
||||
#include <ros/ros.h>
|
||||
|
||||
// ros_control
|
||||
#include <controller_manager/controller_manager.h>
|
||||
|
||||
#include "./../include/steerbot.h"
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "steerbot");
|
||||
ros::NodeHandle nh;
|
||||
|
||||
Steerbot robot;
|
||||
ROS_WARN_STREAM("period: " << robot.getPeriod().toSec());
|
||||
controller_manager::ControllerManager cm(&robot, nh);
|
||||
|
||||
ros::Rate rate(1.0 / robot.getPeriod().toSec());
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
while(ros::ok())
|
||||
{
|
||||
ROS_WARN_STREAM("period: " << robot.getPeriod().toSec());
|
||||
robot.read();
|
||||
cm.update(robot.getTime(), robot.getPeriod());
|
||||
robot.write();
|
||||
rate.sleep();
|
||||
}
|
||||
spinner.stop();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,95 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot name="robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<!--
|
||||
Robot model taken from http://wiki.ros.org/pr2_mechanism/Tutorials/SImple%20URDF-Controller%20Example
|
||||
-->
|
||||
<xacro:include filename="$(find steer_drive_controller)/test/common/urdf/wheel.xacro"/>
|
||||
|
||||
<xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
|
||||
|
||||
<!-- Constants for robot dimensions -->
|
||||
<xacro:property name="length" value="0.5" />
|
||||
<xacro:property name="width" value="0.3" />
|
||||
<xacro:property name="wheel_radius" value="0.11" /> <!-- Link 1 -->
|
||||
<xacro:property name="thickness" value="0.086" /> <!-- Link 2 -->
|
||||
<xacro:property name="axel_offset" value="0.05" /> <!-- Space btw top of beam and the each joint -->
|
||||
<xacro:property name="steer_offset" value="0.02" /> <!-- Link 1 -->
|
||||
|
||||
<!-- Links: inertial,visual,collision -->
|
||||
<link name="base_link">
|
||||
<inertial>
|
||||
<!-- origin is relative -->
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<mass value="5"/>
|
||||
<inertia ixx="5" ixy="0" ixz="0" iyy="5" iyz="0" izz="5"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="${length} ${width} 0.1"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<!-- origin is relative -->
|
||||
<origin xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="${length} ${width} 0.1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
|
||||
<link name="base_footprint">
|
||||
<visual>
|
||||
<geometry>
|
||||
<sphere radius="0.01"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="0.00000001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="base_footprint_joint" type="fixed">
|
||||
<origin xyz="0 0 ${wheel_radius}" rpy="0 0 0"/>
|
||||
<child link="base_link"/>
|
||||
<parent link="base_footprint"/>
|
||||
</joint>
|
||||
|
||||
<!-- joints for steer_drive_controller -->
|
||||
<front_steer name="front" parent="base" radius="${wheel_radius}" thickness="${thickness}"
|
||||
length="${length}" width="${width}" axel_offset="${axel_offset}" steer_height="${wheel_radius+steer_offset}">
|
||||
<origin xyz="${length/2-axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
|
||||
</front_steer>
|
||||
<rear_wheel name="rear" parent="base" radius="${wheel_radius}" thickness="${thickness}">
|
||||
<origin xyz="${-length/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
|
||||
</rear_wheel>
|
||||
|
||||
<!-- Wheels -->
|
||||
<front_wheel_lr name="front_right" parent="base" radius="${wheel_radius}" thickness="${thickness}"
|
||||
length="${length}" width="${width}" axel_offset="${axel_offset}" right_left="1" steer_height="${wheel_radius+steer_offset}">
|
||||
<origin xyz="${length/2-axel_offset} ${width/2+axel_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
|
||||
</front_wheel_lr>
|
||||
<front_wheel_lr name="front_left" parent="base" radius="${wheel_radius}" thickness="${thickness}"
|
||||
length="${length}" width="${width}" axel_offset="${axel_offset}" right_left="-1" steer_height="${wheel_radius+steer_offset}">
|
||||
<origin xyz="${length/2-axel_offset} ${-width/2-axel_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
|
||||
</front_wheel_lr>
|
||||
<rear_wheel_lr name="rear_right" parent="base" radius="${wheel_radius}" thickness="${thickness}">
|
||||
<origin xyz="${-length/2+axel_offset} ${width/2+axel_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
|
||||
</rear_wheel_lr>
|
||||
<rear_wheel_lr name="rear_left" parent="base" radius="${wheel_radius}" thickness="${thickness}">
|
||||
<origin xyz="${-length/2+axel_offset} ${-width/2-axel_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
|
||||
</rear_wheel_lr>
|
||||
|
||||
|
||||
<!-- Colour -->
|
||||
<gazebo reference="base_link">
|
||||
<material>Gazebo/Green</material>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="base_footprint">
|
||||
<material>Gazebo/Purple</material>
|
||||
</gazebo>
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,95 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot name="robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<!--
|
||||
Robot model taken from http://wiki.ros.org/pr2_mechanism/Tutorials/SImple%20URDF-Controller%20Example
|
||||
-->
|
||||
<xacro:include filename="$(find steer_drive_controller)/test/common/urdf/wheel_sphere.xacro"/>
|
||||
|
||||
<xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
|
||||
|
||||
<!-- Constants for robot dimensions -->
|
||||
<xacro:property name="length" value="0.5" />
|
||||
<xacro:property name="width" value="0.3" />
|
||||
<xacro:property name="wheel_radius" value="0.11" /> <!-- Link 1 -->
|
||||
<xacro:property name="thickness" value="0.086" /> <!-- Link 2 -->
|
||||
<xacro:property name="axel_offset" value="0.05" /> <!-- Space btw top of beam and the each joint -->
|
||||
<xacro:property name="steer_offset" value="0.02" /> <!-- Link 1 -->
|
||||
|
||||
<!-- Links: inertial,visual,collision -->
|
||||
<link name="base_link">
|
||||
<inertial>
|
||||
<!-- origin is relative -->
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<mass value="5"/>
|
||||
<inertia ixx="5" ixy="0" ixz="0" iyy="5" iyz="0" izz="5"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="${length} ${width} 0.1"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<!-- origin is relative -->
|
||||
<origin xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="${length} ${width} 0.1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
|
||||
<link name="base_footprint">
|
||||
<visual>
|
||||
<geometry>
|
||||
<sphere radius="0.01"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="0.00000001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="base_footprint_joint" type="fixed">
|
||||
<origin xyz="0 0 ${wheel_radius}" rpy="0 0 0"/>
|
||||
<child link="base_link"/>
|
||||
<parent link="base_footprint"/>
|
||||
</joint>
|
||||
|
||||
<!-- joints for steer_drive_controller -->
|
||||
<front_steer name="front" parent="base" radius="${wheel_radius}" thickness="${thickness}"
|
||||
length="${length}" width="${width}" axel_offset="${axel_offset}" steer_height="${wheel_radius+steer_offset}">
|
||||
<origin xyz="${length/2-axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
|
||||
</front_steer>
|
||||
<rear_wheel name="rear" parent="base" radius="${wheel_radius}" thickness="${thickness}">
|
||||
<origin xyz="${-length/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
|
||||
</rear_wheel>
|
||||
|
||||
<!-- Wheels -->
|
||||
<front_wheel_lr name="front_right" parent="base" radius="${wheel_radius}" thickness="${thickness}"
|
||||
length="${length}" width="${width}" axel_offset="${axel_offset}" right_left="1" steer_height="${wheel_radius+steer_offset}">
|
||||
<origin xyz="${length/2-axel_offset} ${width/2+axel_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
|
||||
</front_wheel_lr>
|
||||
<front_wheel_lr name="front_left" parent="base" radius="${wheel_radius}" thickness="${thickness}"
|
||||
length="${length}" width="${width}" axel_offset="${axel_offset}" right_left="-1" steer_height="${wheel_radius+steer_offset}">
|
||||
<origin xyz="${length/2-axel_offset} ${-width/2-axel_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
|
||||
</front_wheel_lr>
|
||||
<rear_wheel_lr name="rear_right" parent="base" radius="${wheel_radius}" thickness="${thickness}">
|
||||
<origin xyz="${-length/2+axel_offset} ${width/2+axel_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
|
||||
</rear_wheel_lr>
|
||||
<rear_wheel_lr name="rear_left" parent="base" radius="${wheel_radius}" thickness="${thickness}">
|
||||
<origin xyz="${-length/2+axel_offset} ${-width/2-axel_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
|
||||
</rear_wheel_lr>
|
||||
|
||||
|
||||
<!-- Colour -->
|
||||
<gazebo reference="base_link">
|
||||
<material>Gazebo/Green</material>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="base_footprint">
|
||||
<material>Gazebo/Purple</material>
|
||||
</gazebo>
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,206 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<xacro:property name="steer_radius" value="0.05" /> <!-- Link 1 -->
|
||||
<xacro:property name="steer_thickness" value="0.02" /> <!-- Link 1 -->
|
||||
<xacro:property name="steer_effort" value="10.0"/>
|
||||
<xacro:property name="steer_velocity" value="5.0"/>
|
||||
|
||||
<xacro:macro name="front_steer" params="name parent radius thickness length width axel_offset steer_height *origin">
|
||||
<link name="${name}_steer_link">
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder radius="${steer_radius}" length="${steer_thickness}"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder radius="${steer_radius}" length="${steer_thickness}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="${name}_steer_joint" type="revolute">
|
||||
<parent link="${parent}_link"/>
|
||||
<child link="${name}_steer_link"/>
|
||||
<origin xyz="${length/2-axel_offset} 0 ${steer_height}" rpy="0 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="${steer_effort}"
|
||||
lower="${-180.0 * deg_to_rad}" upper="${180.0 * deg_to_rad}"
|
||||
velocity="${steer_velocity}"/>
|
||||
</joint>
|
||||
|
||||
<gazebo reference="${name}_steer_link">
|
||||
<material>Gazebo/Yellow</material>
|
||||
</gazebo>
|
||||
</xacro:macro>
|
||||
|
||||
<xacro:macro name="front_wheel_lr" params="name parent radius thickness length width axel_offset right_left steer_height *origin">
|
||||
<link name="${name}_steer_link">
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder radius="${steer_radius}" length="${steer_thickness}"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder radius="${steer_radius}" length="${steer_thickness}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="${name}_steer_joint" type="revolute">
|
||||
<parent link="${parent}_link"/>
|
||||
<child link="${name}_steer_link"/>
|
||||
<origin xyz="${length/2-axel_offset} ${right_left*(width/2+axel_offset)} ${steer_height}" rpy="0 0 0"/>
|
||||
<!--xacro:insert_block name="origin"/-->
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="${steer_effort}"
|
||||
lower="${-180.0 * deg_to_rad}" upper="${180.0 * deg_to_rad}"
|
||||
velocity="${steer_velocity}"/>
|
||||
</joint>
|
||||
|
||||
<link name="${name}_wheel_link">
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder radius="${wheel_radius}" length="${thickness}"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder radius="${wheel_radius}" length="${thickness}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="${name}_wheel_joint" type="continuous">
|
||||
<parent link="${name}_steer_link"/>
|
||||
<child link="${name}_wheel_link"/>
|
||||
<origin xyz="0 0 ${-steer_height}" rpy="${-90 * deg_to_rad} 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
</joint>
|
||||
|
||||
<!-- Transmission is important to link the joints and the controller -->
|
||||
<transmission name="${name}_steer_joint_trans" type="SimpleTransmission">
|
||||
<actuator name="${name}_steer_joint_motor"/>
|
||||
<joint name="${name}_steer_joint"/>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<motorTorqueConstant>1</motorTorqueConstant>
|
||||
</transmission>
|
||||
<transmission name="${name}_wheel_joint_trans" type="SimpleTransmission">
|
||||
<actuator name="${name}_wheel_joint_motor"/>
|
||||
<joint name="${name}_wheel_joint"/>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<motorTorqueConstant>1</motorTorqueConstant>
|
||||
</transmission>
|
||||
|
||||
<gazebo reference="${name}_steer_link">
|
||||
<material>Gazebo/Blue</material>
|
||||
</gazebo>
|
||||
<gazebo reference="${name}_wheel_link">
|
||||
<material>Gazebo/Red</material>
|
||||
</gazebo>
|
||||
</xacro:macro>
|
||||
|
||||
<xacro:macro name="rear_wheel" params="name parent radius thickness *origin">
|
||||
<link name="${name}_wheel_link">
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder radius="${wheel_radius}" length="${thickness}"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder radius="${wheel_radius}" length="${thickness}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="${name}_wheel_joint" type="continuous">
|
||||
<parent link="${parent}_link"/>
|
||||
<child link="${name}_wheel_link"/>
|
||||
<xacro:insert_block name="origin"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
</joint>
|
||||
|
||||
<!-- Transmission is important to link the joints and the controller -->
|
||||
<transmission name="${name}_wheel_joint_trans" type="SimpleTransmission">
|
||||
<actuator name="${name}_wheel_joint_motor"/>
|
||||
<joint name="${name}_wheel_joint"/>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<motorTorqueConstant>1</motorTorqueConstant>
|
||||
</transmission>
|
||||
|
||||
<gazebo reference="${name}_wheel_link">
|
||||
<material>Gazebo/Yellow</material>
|
||||
</gazebo>
|
||||
</xacro:macro>
|
||||
|
||||
<xacro:macro name="rear_wheel_lr" params="name parent radius thickness *origin">
|
||||
<link name="${name}_wheel_link">
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder radius="${wheel_radius}" length="${thickness}"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder radius="${wheel_radius}" length="${thickness}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="${name}_wheel_joint" type="continuous">
|
||||
<parent link="${parent}_link"/>
|
||||
<child link="${name}_wheel_link"/>
|
||||
<xacro:insert_block name="origin"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
</joint>
|
||||
|
||||
<!-- Transmission is important to link the joints and the controller -->
|
||||
<transmission name="${name}_wheel_joint_trans" type="SimpleTransmission">
|
||||
<actuator name="${name}_wheel_joint_motor"/>
|
||||
<joint name="${name}_wheel_joint"/>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<motorTorqueConstant>1</motorTorqueConstant>
|
||||
</transmission>
|
||||
|
||||
<gazebo reference="${name}_wheel_link">
|
||||
<material>Gazebo/Red</material>
|
||||
</gazebo>
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
@@ -0,0 +1,206 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<xacro:property name="steer_radius" value="0.05" /> <!-- Link 1 -->
|
||||
<xacro:property name="steer_thickness" value="0.02" /> <!-- Link 1 -->
|
||||
<xacro:property name="steer_effort" value="10.0"/>
|
||||
<xacro:property name="steer_velocity" value="5.0"/>
|
||||
|
||||
<xacro:macro name="front_steer" params="name parent radius thickness length width axel_offset steer_height *origin">
|
||||
<link name="${name}_steer_link">
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder radius="${steer_radius}" length="${steer_thickness}"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder radius="${steer_radius}" length="${steer_thickness}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="${name}_steer_joint" type="revolute">
|
||||
<parent link="${parent}_link"/>
|
||||
<child link="${name}_steer_link"/>
|
||||
<origin xyz="${length/2-axel_offset} 0 ${steer_height}" rpy="0 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="${steer_effort}"
|
||||
lower="${-180.0 * deg_to_rad}" upper="${180.0 * deg_to_rad}"
|
||||
velocity="${steer_velocity}"/>
|
||||
</joint>
|
||||
|
||||
<gazebo reference="${name}_steer_link">
|
||||
<material>Gazebo/Yellow</material>
|
||||
</gazebo>
|
||||
</xacro:macro>
|
||||
|
||||
<xacro:macro name="front_wheel_lr" params="name parent radius thickness length width axel_offset right_left steer_height *origin">
|
||||
<link name="${name}_steer_link">
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder radius="${steer_radius}" length="${steer_thickness}"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder radius="${steer_radius}" length="${steer_thickness}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="${name}_steer_joint" type="revolute">
|
||||
<parent link="${parent}_link"/>
|
||||
<child link="${name}_steer_link"/>
|
||||
<origin xyz="${length/2-axel_offset} ${right_left*(width/2+axel_offset)} ${steer_height}" rpy="0 0 0"/>
|
||||
<!--xacro:insert_block name="origin"/-->
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="${steer_effort}"
|
||||
lower="${-180.0 * deg_to_rad}" upper="${180.0 * deg_to_rad}"
|
||||
velocity="${steer_velocity}"/>
|
||||
</joint>
|
||||
|
||||
<link name="${name}_wheel_link">
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="${wheel_radius}"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="${wheel_radius}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="${name}_wheel_joint" type="continuous">
|
||||
<parent link="${name}_steer_link"/>
|
||||
<child link="${name}_wheel_link"/>
|
||||
<origin xyz="0 0 ${-steer_height}" rpy="${-90 * deg_to_rad} 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
</joint>
|
||||
|
||||
<!-- Transmission is important to link the joints and the controller -->
|
||||
<transmission name="${name}_steer_joint_trans" type="SimpleTransmission">
|
||||
<actuator name="${name}_steer_joint_motor"/>
|
||||
<joint name="${name}_steer_joint"/>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<motorTorqueConstant>1</motorTorqueConstant>
|
||||
</transmission>
|
||||
<transmission name="${name}_wheel_joint_trans" type="SimpleTransmission">
|
||||
<actuator name="${name}_wheel_joint_motor"/>
|
||||
<joint name="${name}_wheel_joint"/>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<motorTorqueConstant>1</motorTorqueConstant>
|
||||
</transmission>
|
||||
|
||||
<gazebo reference="${name}_steer_link">
|
||||
<material>Gazebo/Blue</material>
|
||||
</gazebo>
|
||||
<gazebo reference="${name}_wheel_link">
|
||||
<material>Gazebo/Red</material>
|
||||
</gazebo>
|
||||
</xacro:macro>
|
||||
|
||||
<xacro:macro name="rear_wheel" params="name parent radius thickness *origin">
|
||||
<link name="${name}_wheel_link">
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="${wheel_radius}"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="${wheel_radius}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="${name}_wheel_joint" type="continuous">
|
||||
<parent link="${parent}_link"/>
|
||||
<child link="${name}_wheel_link"/>
|
||||
<xacro:insert_block name="origin"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
</joint>
|
||||
|
||||
<!-- Transmission is important to link the joints and the controller -->
|
||||
<transmission name="${name}_wheel_joint_trans" type="SimpleTransmission">
|
||||
<actuator name="${name}_wheel_joint_motor"/>
|
||||
<joint name="${name}_wheel_joint"/>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<motorTorqueConstant>1</motorTorqueConstant>
|
||||
</transmission>
|
||||
|
||||
<gazebo reference="${name}_wheel_link">
|
||||
<material>Gazebo/Yellow</material>
|
||||
</gazebo>
|
||||
</xacro:macro>
|
||||
|
||||
<xacro:macro name="rear_wheel_lr" params="name parent radius thickness *origin">
|
||||
<link name="${name}_wheel_link">
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="${wheel_radius}"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="${wheel_radius}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="${name}_wheel_joint" type="continuous">
|
||||
<parent link="${parent}_link"/>
|
||||
<child link="${name}_wheel_link"/>
|
||||
<xacro:insert_block name="origin"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
</joint>
|
||||
|
||||
<!-- Transmission is important to link the joints and the controller -->
|
||||
<transmission name="${name}_wheel_joint_trans" type="SimpleTransmission">
|
||||
<actuator name="${name}_wheel_joint_motor"/>
|
||||
<joint name="${name}_wheel_joint"/>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<motorTorqueConstant>1</motorTorqueConstant>
|
||||
</transmission>
|
||||
|
||||
<gazebo reference="${name}_wheel_link">
|
||||
<material>Gazebo/Red</material>
|
||||
</gazebo>
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
@@ -0,0 +1,18 @@
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find steer_drive_controller)/test/common/launch/steer_drive_common.launch" />
|
||||
<!-- Do not specific an odom_frame_id -->
|
||||
<!-- <rosparam>
|
||||
steerbot_controller:
|
||||
odom_frame_id: odom
|
||||
</rosparam> -->
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="steer_drive_controller_default_odom_frame_test"
|
||||
pkg="steer_drive_controller"
|
||||
type="steer_drive_controller_default_odom_frame_test"
|
||||
time-limit="80.0">
|
||||
<remap from="cmd_vel" to="steerbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="steerbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,73 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// Copyright (C) 2015, Locus Robotics Corp.
|
||||
//
|
||||
// Redistribution and use in source and binary forms, with or without
|
||||
// modification, are permitted provided that the following conditions are met:
|
||||
// * Redistributions of source code must retain the above copyright notice,
|
||||
// this list of conditions and the following disclaimer.
|
||||
// * Redistributions in binary form must reproduce the above copyright
|
||||
// notice, this list of conditions and the following disclaimer in the
|
||||
// documentation and/or other materials provided with the distribution.
|
||||
// * Neither the name of PAL Robotics, Inc. nor the names of its
|
||||
// contributors may be used to endorse or promote products derived from
|
||||
// this software without specific prior written permission.
|
||||
//
|
||||
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
// POSSIBILITY OF SUCH DAMAGE.
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/// \author Eric Tappan
|
||||
|
||||
#include "../common/include/test_common.h"
|
||||
#include <tf/transform_listener.h>
|
||||
|
||||
// TEST CASES
|
||||
TEST_F(SteerDriveControllerTest, testOdomFrame)
|
||||
{
|
||||
// wait for ROS
|
||||
while(!isControllerAlive())
|
||||
{
|
||||
ros::Duration(0.1).sleep();
|
||||
}
|
||||
// set up tf listener
|
||||
tf::TransformListener listener;
|
||||
ros::Duration(2.0).sleep();
|
||||
// check the original odom frame doesn't exist
|
||||
EXPECT_TRUE(listener.frameExists("odom"));
|
||||
}
|
||||
|
||||
TEST_F(SteerDriveControllerTest, testOdomTopic)
|
||||
{
|
||||
// wait for ROS
|
||||
while(!isControllerAlive())
|
||||
{
|
||||
ros::Duration(0.1).sleep();
|
||||
}
|
||||
|
||||
// get an odom message
|
||||
nav_msgs::Odometry odom_msg = getLastOdom();
|
||||
// check its frame_id
|
||||
ASSERT_STREQ(odom_msg.header.frame_id.c_str(), "odom");
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
ros::init(argc, argv, "steer_drive_controller_default_odom_frame_test");
|
||||
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
int ret = RUN_ALL_TESTS();
|
||||
spinner.stop();
|
||||
ros::shutdown();
|
||||
return ret;
|
||||
}
|
||||
@@ -0,0 +1,57 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// Copyright (C) 2013, PAL Robotics S.L.
|
||||
//
|
||||
// Redistribution and use in source and binary forms, with or without
|
||||
// modification, are permitted provided that the following conditions are met:
|
||||
// * Redistributions of source code must retain the above copyright notice,
|
||||
// this list of conditions and the following disclaimer.
|
||||
// * Redistributions in binary form must reproduce the above copyright
|
||||
// notice, this list of conditions and the following disclaimer in the
|
||||
// documentation and/or other materials provided with the distribution.
|
||||
// * Neither the name of PAL Robotics, Inc. nor the names of its
|
||||
// contributors may be used to endorse or promote products derived from
|
||||
// this software without specific prior written permission.
|
||||
//
|
||||
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
// POSSIBILITY OF SUCH DAMAGE.
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/// \author Paul Mathieu
|
||||
|
||||
#include "../common/include/test_common.h"
|
||||
|
||||
// TEST CASES
|
||||
TEST_F(SteerDriveControllerTest, testWrongJointName)
|
||||
{
|
||||
// the controller should never be alive
|
||||
int secs = 0;
|
||||
while(!isControllerAlive() && secs < 5)
|
||||
{
|
||||
ros::Duration(1.0).sleep();
|
||||
secs++;
|
||||
}
|
||||
// give up and assume controller load failure after 5 seconds
|
||||
EXPECT_GE(secs, 5);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
ros::init(argc, argv, "steer_drive_controller_fail_test");
|
||||
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
int ret = RUN_ALL_TESTS();
|
||||
spinner.stop();
|
||||
ros::shutdown();
|
||||
return ret;
|
||||
}
|
||||
@@ -0,0 +1,16 @@
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find steer_drive_controller)/test/common/launch/steer_drive_common.launch" />
|
||||
|
||||
<!-- Override with wrong controller configuration -->
|
||||
<rosparam command="load" file="$(find steer_drive_controller)/test/steer_drive_controller_fail_test/steerbot_wrong.yaml" />
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="steer_drive_controller_fail_test"
|
||||
pkg="steer_drive_controller"
|
||||
type="steer_drive_controller_fail_test"
|
||||
time-limit="10.0">
|
||||
<remap from="cmd_vel" to="steerbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="steerbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,8 @@
|
||||
steerbot_controller:
|
||||
type: "steer_drive_controller/SteerDriveController"
|
||||
rear_wheel : 'this_joint_does_not_exist'
|
||||
front_steer : 'front_steer_joint'
|
||||
publish_rate: 50.0 # defaults to 50
|
||||
pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
|
||||
twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
|
||||
cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests
|
||||
@@ -0,0 +1,16 @@
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find steer_drive_controller)/test/common/launch/steer_drive_common.launch" />
|
||||
|
||||
<!-- Load diff-drive limits -->
|
||||
<rosparam command="load" file="$(find steer_drive_controller)/test/steer_drive_controller_limits_test/steerbot_limits.yaml" />
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="steer_drive_controller_limits_test"
|
||||
pkg="steer_drive_controller"
|
||||
type="steer_drive_controller_limits_test"
|
||||
time-limit="80.0">
|
||||
<remap from="cmd_vel" to="steerbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="steerbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,242 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// Copyright (C) 2013, PAL Robotics S.L.
|
||||
//
|
||||
// Redistribution and use in source and binary forms, with or without
|
||||
// modification, are permitted provided that the following conditions are met:
|
||||
// * Redistributions of source code must retain the above copyright notice,
|
||||
// this list of conditions and the following disclaimer.
|
||||
// * Redistributions in binary form must reproduce the above copyright
|
||||
// notice, this list of conditions and the following disclaimer in the
|
||||
// documentation and/or other materials provided with the distribution.
|
||||
// * Neither the name of PAL Robotics, Inc. nor the names of its
|
||||
// contributors may be used to endorse or promote products derived from
|
||||
// this software without specific prior written permission.
|
||||
//
|
||||
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
// POSSIBILITY OF SUCH DAMAGE.
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/// \author Paul Mathieu
|
||||
|
||||
#include "../common/include/test_common.h"
|
||||
|
||||
// TEST CASES
|
||||
TEST_F(SteerDriveControllerTest, testLinearJerkLimits)
|
||||
{
|
||||
// wait for ROS
|
||||
while(!isControllerAlive())
|
||||
{
|
||||
ros::Duration(0.1).sleep();
|
||||
}
|
||||
// zero everything before test
|
||||
geometry_msgs::Twist cmd_vel;
|
||||
cmd_vel.linear.x = 0.0;
|
||||
cmd_vel.angular.z = 0.0;
|
||||
publish(cmd_vel);
|
||||
ros::Duration(2.0).sleep();
|
||||
// get initial odom
|
||||
nav_msgs::Odometry old_odom = getLastOdom();
|
||||
// send a big command
|
||||
cmd_vel.linear.x = 10.0;
|
||||
publish(cmd_vel);
|
||||
// wait for a while
|
||||
ros::Duration(0.5).sleep();
|
||||
|
||||
nav_msgs::Odometry new_odom = getLastOdom();
|
||||
|
||||
// check if the robot speed is now 0.37m.s-1
|
||||
EXPECT_NEAR(new_odom.twist.twist.linear.x, 0.37, JERK_LINEAR_VELOCITY_TOLERANCE);
|
||||
EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), EPS);
|
||||
|
||||
cmd_vel.linear.x = 0.0;
|
||||
publish(cmd_vel);
|
||||
}
|
||||
|
||||
TEST_F(SteerDriveControllerTest, testLinearAccelerationLimits)
|
||||
{
|
||||
// wait for ROS
|
||||
while(!isControllerAlive())
|
||||
{
|
||||
ros::Duration(0.1).sleep();
|
||||
}
|
||||
// zero everything before test
|
||||
geometry_msgs::Twist cmd_vel;
|
||||
cmd_vel.linear.x = 0.0;
|
||||
cmd_vel.angular.z = 0.0;
|
||||
publish(cmd_vel);
|
||||
ros::Duration(2.0).sleep();
|
||||
// get initial odom
|
||||
nav_msgs::Odometry old_odom = getLastOdom();
|
||||
// send a big command
|
||||
cmd_vel.linear.x = 10.0;
|
||||
publish(cmd_vel);
|
||||
// wait for a while
|
||||
ros::Duration(0.5).sleep();
|
||||
|
||||
nav_msgs::Odometry new_odom = getLastOdom();
|
||||
|
||||
// check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s
|
||||
EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), 0.5 + VELOCITY_TOLERANCE);
|
||||
EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), EPS);
|
||||
|
||||
cmd_vel.linear.x = 0.0;
|
||||
publish(cmd_vel);
|
||||
}
|
||||
|
||||
TEST_F(SteerDriveControllerTest, testLinearVelocityLimits)
|
||||
{
|
||||
// wait for ROS
|
||||
while(!isControllerAlive())
|
||||
{
|
||||
ros::Duration(0.1).sleep();
|
||||
}
|
||||
// zero everything before test
|
||||
geometry_msgs::Twist cmd_vel;
|
||||
cmd_vel.linear.x = 0.0;
|
||||
cmd_vel.angular.z = 0.0;
|
||||
publish(cmd_vel);
|
||||
ros::Duration(2.0).sleep();
|
||||
// get initial odom
|
||||
nav_msgs::Odometry old_odom = getLastOdom();
|
||||
// send a big command
|
||||
cmd_vel.linear.x = 10.0;
|
||||
publish(cmd_vel);
|
||||
// wait for a while
|
||||
ros::Duration(5.0).sleep();
|
||||
|
||||
nav_msgs::Odometry new_odom = getLastOdom();
|
||||
|
||||
// check if the robot speed is now 1.0 m.s-1, the limit
|
||||
EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), 1.0 + VELOCITY_TOLERANCE);
|
||||
EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), EPS);
|
||||
|
||||
cmd_vel.linear.x = 0.0;
|
||||
publish(cmd_vel);
|
||||
}
|
||||
|
||||
TEST_F(SteerDriveControllerTest, testAngularJerkLimits)
|
||||
{
|
||||
// wait for ROS
|
||||
while(!isControllerAlive())
|
||||
{
|
||||
ros::Duration(0.1).sleep();
|
||||
}
|
||||
// zero everything before test
|
||||
geometry_msgs::Twist cmd_vel;
|
||||
cmd_vel.linear.x = 0.0;
|
||||
cmd_vel.angular.z = 0.0;
|
||||
publish(cmd_vel);
|
||||
ros::Duration(2.0).sleep();
|
||||
// get initial odom
|
||||
nav_msgs::Odometry old_odom = getLastOdom();
|
||||
// send a big command
|
||||
cmd_vel.angular.z = 10.0;
|
||||
// send linear command too
|
||||
// because sending only angular command doesn't actuate wheels for steer drive mechanism
|
||||
cmd_vel.linear.x = 0.1;
|
||||
publish(cmd_vel);
|
||||
// wait for a while
|
||||
ros::Duration(0.5).sleep();
|
||||
|
||||
nav_msgs::Odometry new_odom = getLastOdom();
|
||||
|
||||
// check if the robot speed is now 0.18rad.s-1
|
||||
EXPECT_NEAR(new_odom.twist.twist.angular.z, 0.18, JERK_ANGULAR_VELOCITY_TOLERANCE);
|
||||
// check if the robot speed is now 0.1m.s-1
|
||||
EXPECT_NEAR(new_odom.twist.twist.linear.x, 0.1, VELOCITY_TOLERANCE);
|
||||
|
||||
cmd_vel.angular.z = 0.0;
|
||||
publish(cmd_vel);
|
||||
}
|
||||
|
||||
TEST_F(SteerDriveControllerTest, testAngularAccelerationLimits)
|
||||
{
|
||||
// wait for ROS
|
||||
while(!isControllerAlive())
|
||||
{
|
||||
ros::Duration(0.1).sleep();
|
||||
}
|
||||
// zero everything before test
|
||||
geometry_msgs::Twist cmd_vel;
|
||||
cmd_vel.linear.x = 0.0;
|
||||
cmd_vel.angular.z = 0.0;
|
||||
publish(cmd_vel);
|
||||
ros::Duration(2.0).sleep();
|
||||
// get initial odom
|
||||
nav_msgs::Odometry old_odom = getLastOdom();
|
||||
// send a big command
|
||||
cmd_vel.angular.z = 10.0;
|
||||
// send linear command too
|
||||
// because sending only angular command doesn't actuate wheels for steer drive mechanism
|
||||
cmd_vel.linear.x = 0.1;
|
||||
publish(cmd_vel);
|
||||
// wait for a while
|
||||
ros::Duration(0.5).sleep();
|
||||
|
||||
nav_msgs::Odometry new_odom = getLastOdom();
|
||||
|
||||
// check if the robot speed is now 0.25rad.s-1, which is 0.5.s-2 * 0.5s
|
||||
EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), 0.25 + VELOCITY_TOLERANCE);
|
||||
// check if the robot speed is now 0.1m.s-1
|
||||
EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), 0.1 + VELOCITY_TOLERANCE);
|
||||
|
||||
cmd_vel.angular.z = 0.0;
|
||||
publish(cmd_vel);
|
||||
}
|
||||
|
||||
TEST_F(SteerDriveControllerTest, testAngularVelocityLimits)
|
||||
{
|
||||
// wait for ROS
|
||||
while(!isControllerAlive())
|
||||
{
|
||||
ros::Duration(0.1).sleep();
|
||||
}
|
||||
// zero everything before test
|
||||
geometry_msgs::Twist cmd_vel;
|
||||
cmd_vel.linear.x = 0.0;
|
||||
cmd_vel.angular.z = 0.0;
|
||||
publish(cmd_vel);
|
||||
ros::Duration(2.0).sleep();
|
||||
// get initial odom
|
||||
nav_msgs::Odometry old_odom = getLastOdom();
|
||||
cmd_vel.angular.z = 10.0;
|
||||
// send linear command too
|
||||
// because sending only angular command doesn't actuate wheels for steer drive mechanism
|
||||
cmd_vel.linear.x = 0.1;
|
||||
publish(cmd_vel);
|
||||
// wait for a while
|
||||
ros::Duration(5.0).sleep();
|
||||
|
||||
nav_msgs::Odometry new_odom = getLastOdom();
|
||||
|
||||
// check if the robot speed is now 0.5rad.s-1, the limit
|
||||
EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), 0.5 + ANGULAR_VELOCITY_TOLERANCE);
|
||||
// check if the robot speed is now 0.1m.s-1
|
||||
EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), 0.1 + VELOCITY_TOLERANCE);
|
||||
|
||||
cmd_vel.angular.z = 0.0;
|
||||
publish(cmd_vel);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
ros::init(argc, argv, "steer_drive_controller_limits_test");
|
||||
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
//ros::Duration(0.5).sleep();
|
||||
int ret = RUN_ALL_TESTS();
|
||||
spinner.stop();
|
||||
ros::shutdown();
|
||||
return ret;
|
||||
}
|
||||
@@ -0,0 +1,19 @@
|
||||
steerbot_controller:
|
||||
linear:
|
||||
x:
|
||||
has_velocity_limits: true
|
||||
min_velocity: -0.5
|
||||
max_velocity: 1.0
|
||||
has_acceleration_limits: true
|
||||
min_acceleration: -0.5
|
||||
max_acceleration: 1.0
|
||||
has_jerk_limits: true
|
||||
max_jerk: 5.0
|
||||
angular:
|
||||
z:
|
||||
has_velocity_limits: true
|
||||
max_velocity: 0.5
|
||||
has_acceleration_limits: true
|
||||
max_acceleration: 0.5
|
||||
has_jerk_limits: true
|
||||
max_jerk: 2.5
|
||||
@@ -0,0 +1,14 @@
|
||||
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find steer_drive_controller)/test/common/launch/steer_drive_common.launch" />
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="steer_drive_controller_nan_test"
|
||||
pkg="steer_drive_controller"
|
||||
type="steer_drive_controller_nan_test"
|
||||
time-limit="80.0">
|
||||
<remap from="cmd_vel" to="steerbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="steerbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,92 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// Copyright (C) 2014, PAL Robotics S.L.
|
||||
//
|
||||
// Redistribution and use in source and binary forms, with or without
|
||||
// modification, are permitted provided that the following conditions are met:
|
||||
// * Redistributions of source code must retain the above copyright notice,
|
||||
// this list of conditions and the following disclaimer.
|
||||
// * Redistributions in binary form must reproduce the above copyright
|
||||
// notice, this list of conditions and the following disclaimer in the
|
||||
// documentation and/or other materials provided with the distribution.
|
||||
// * Neither the name of PAL Robotics, Inc. nor the names of its
|
||||
// contributors may be used to endorse or promote products derived from
|
||||
// this software without specific prior written permission.
|
||||
//
|
||||
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
// POSSIBILITY OF SUCH DAMAGE.
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/// \author Enrique Fernandez
|
||||
|
||||
#include "../common/include/test_common.h"
|
||||
|
||||
// NaN
|
||||
#include <limits>
|
||||
|
||||
// TEST CASES
|
||||
TEST_F(SteerDriveControllerTest, testNaN)
|
||||
{
|
||||
// wait for ROS
|
||||
while(!isControllerAlive())
|
||||
{
|
||||
ros::Duration(0.1).sleep();
|
||||
}
|
||||
// zero everything before test
|
||||
geometry_msgs::Twist cmd_vel;
|
||||
cmd_vel.linear.x = 0.0;
|
||||
cmd_vel.angular.z = 0.0;
|
||||
publish(cmd_vel);
|
||||
ros::Duration(2.0).sleep();
|
||||
|
||||
// send a command
|
||||
cmd_vel.linear.x = 0.1;
|
||||
ros::Duration(2.0).sleep();
|
||||
|
||||
// stop robot (will generate NaN)
|
||||
stop();
|
||||
ros::Duration(2.0).sleep();
|
||||
|
||||
nav_msgs::Odometry odom = getLastOdom();
|
||||
|
||||
EXPECT_NE(std::isnan(odom.twist.twist.linear.x), true);
|
||||
EXPECT_NE(std::isnan(odom.twist.twist.angular.z), true);
|
||||
EXPECT_NE(std::isnan(odom.pose.pose.position.x), true);
|
||||
EXPECT_NE(std::isnan(odom.pose.pose.position.y), true);
|
||||
EXPECT_NE(std::isnan(odom.pose.pose.orientation.z), true);
|
||||
EXPECT_NE(std::isnan(odom.pose.pose.orientation.w), true);
|
||||
|
||||
// start robot
|
||||
start();
|
||||
ros::Duration(2.0).sleep();
|
||||
|
||||
odom = getLastOdom();
|
||||
|
||||
EXPECT_NE(std::isnan(odom.twist.twist.linear.x), true);
|
||||
EXPECT_NE(std::isnan(odom.twist.twist.angular.z), true);
|
||||
EXPECT_NE(std::isnan(odom.pose.pose.position.x), true);
|
||||
EXPECT_NE(std::isnan(odom.pose.pose.position.y), true);
|
||||
EXPECT_NE(std::isnan(odom.pose.pose.orientation.z), true);
|
||||
EXPECT_NE(std::isnan(odom.pose.pose.orientation.w), true);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
ros::init(argc, argv, "steer_drive_controller_nan_test");
|
||||
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
int ret = RUN_ALL_TESTS();
|
||||
spinner.stop();
|
||||
ros::shutdown();
|
||||
return ret;
|
||||
}
|
||||
@@ -0,0 +1,16 @@
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find steer_drive_controller)/test/common/launch/steer_drive_common.launch" />
|
||||
|
||||
<!-- Override with wrong controller configuration -->
|
||||
<rosparam command="load" file="$(find steer_drive_controller)/test/steer_drive_controller_no_steer_test/steerbot_no_steer.yaml" />
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="steer_drive_controller_no_steer_test"
|
||||
pkg="steer_drive_controller"
|
||||
type="steer_drive_controller_fail_test"
|
||||
time-limit="10.0">
|
||||
<remap from="cmd_vel" to="steerbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="steerbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,8 @@
|
||||
steerbot_controller:
|
||||
type: "steer_drive_controller/SteerDriveController"
|
||||
rear_wheel : 'rear_wheel_joint'
|
||||
front_steer : ''
|
||||
publish_rate: 50.0 # defaults to 50
|
||||
pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
|
||||
twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
|
||||
cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests
|
||||
@@ -0,0 +1,16 @@
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find steer_drive_controller)/test/common/launch/steer_drive_common.launch" />
|
||||
|
||||
<!-- Override with wrong controller configuration -->
|
||||
<rosparam command="load" file="$(find steer_drive_controller)/test/steer_drive_controller_no_wheel_test/steerbot_no_wheel.yaml" />
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="steer_drive_controller_no_wheel_test"
|
||||
pkg="steer_drive_controller"
|
||||
type="steer_drive_controller_fail_test"
|
||||
time-limit="10.0">
|
||||
<remap from="cmd_vel" to="steerbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="steerbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,8 @@
|
||||
steerbot_controller:
|
||||
type: "steer_drive_controller/SteerDriveController"
|
||||
rear_wheel : ''
|
||||
front_steer : 'front_steer_joint'
|
||||
publish_rate: 50.0 # defaults to 50
|
||||
pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
|
||||
twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
|
||||
cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests
|
||||
@@ -0,0 +1,19 @@
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find steer_drive_controller)/test/common/launch/steer_drive_common.launch" />
|
||||
|
||||
<!-- Set odom_frame_id to something new -->
|
||||
<rosparam>
|
||||
steerbot_controller:
|
||||
odom_frame_id: new_odom
|
||||
</rosparam>
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="steer_drive_controller_odom_frame_test"
|
||||
pkg="steer_drive_controller"
|
||||
type="steer_drive_controller_odom_frame_test"
|
||||
time-limit="80.0">
|
||||
<remap from="cmd_vel" to="steerbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="steerbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,88 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// Copyright (C) 2015, Locus Robotics Corp.
|
||||
//
|
||||
// Redistribution and use in source and binary forms, with or without
|
||||
// modification, are permitted provided that the following conditions are met:
|
||||
// * Redistributions of source code must retain the above copyright notice,
|
||||
// this list of conditions and the following disclaimer.
|
||||
// * Redistributions in binary form must reproduce the above copyright
|
||||
// notice, this list of conditions and the following disclaimer in the
|
||||
// documentation and/or other materials provided with the distribution.
|
||||
// * Neither the name of PAL Robotics, Inc. nor the names of its
|
||||
// contributors may be used to endorse or promote products derived from
|
||||
// this software without specific prior written permission.
|
||||
//
|
||||
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
// POSSIBILITY OF SUCH DAMAGE.
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/// \author Eric Tappan
|
||||
|
||||
#include "../common/include/test_common.h"
|
||||
#include <tf/transform_listener.h>
|
||||
|
||||
// TEST CASES
|
||||
TEST_F(SteerDriveControllerTest, testNoOdomFrame)
|
||||
{
|
||||
// wait for ROS
|
||||
while(!isControllerAlive())
|
||||
{
|
||||
ros::Duration(0.1).sleep();
|
||||
}
|
||||
// set up tf listener
|
||||
tf::TransformListener listener;
|
||||
ros::Duration(2.0).sleep();
|
||||
// check the original odom frame doesn't exist
|
||||
EXPECT_FALSE(listener.frameExists("odom"));
|
||||
}
|
||||
|
||||
TEST_F(SteerDriveControllerTest, testNewOdomFrame)
|
||||
{
|
||||
// wait for ROS
|
||||
while(!isControllerAlive())
|
||||
{
|
||||
ros::Duration(0.1).sleep();
|
||||
}
|
||||
// set up tf listener
|
||||
tf::TransformListener listener;
|
||||
ros::Duration(2.0).sleep();
|
||||
// check the new_odom frame does exist
|
||||
EXPECT_TRUE(listener.frameExists("new_odom"));
|
||||
}
|
||||
|
||||
TEST_F(SteerDriveControllerTest, testOdomTopic)
|
||||
{
|
||||
// wait for ROS
|
||||
while(!isControllerAlive())
|
||||
{
|
||||
ros::Duration(0.1).sleep();
|
||||
}
|
||||
|
||||
ros::Duration(2.0).sleep();
|
||||
// get an odom message
|
||||
nav_msgs::Odometry odom_msg = getLastOdom();
|
||||
// check its frame_id
|
||||
ASSERT_STREQ(odom_msg.header.frame_id.c_str(), "new_odom");
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
ros::init(argc, argv, "steer_drive_controller_odom_frame_test");
|
||||
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
int ret = RUN_ALL_TESTS();
|
||||
spinner.stop();
|
||||
ros::shutdown();
|
||||
return ret;
|
||||
}
|
||||
@@ -0,0 +1,18 @@
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find steer_drive_controller)/test/common/launch/steer_drive_common.launch" />
|
||||
<!-- Make sure to disable odom tf -->
|
||||
<rosparam>
|
||||
steerbot_controller:
|
||||
enable_odom_tf: False
|
||||
</rosparam>
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="steer_drive_controller_odom_tf_test"
|
||||
pkg="steer_drive_controller"
|
||||
type="steer_drive_controller_odom_tf_test"
|
||||
time-limit="80.0">
|
||||
<remap from="cmd_vel" to="steerbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="steerbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,59 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// Copyright (C) 2014, PAL Robotics S.L.
|
||||
//
|
||||
// Redistribution and use in source and binary forms, with or without
|
||||
// modification, are permitted provided that the following conditions are met:
|
||||
// * Redistributions of source code must retain the above copyright notice,
|
||||
// this list of conditions and the following disclaimer.
|
||||
// * Redistributions in binary form must reproduce the above copyright
|
||||
// notice, this list of conditions and the following disclaimer in the
|
||||
// documentation and/or other materials provided with the distribution.
|
||||
// * Neither the name of PAL Robotics, Inc. nor the names of its
|
||||
// contributors may be used to endorse or promote products derived from
|
||||
// this software without specific prior written permission.
|
||||
//
|
||||
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
// POSSIBILITY OF SUCH DAMAGE.
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/// \author Bence Magyar
|
||||
|
||||
#include "../common/include/test_common.h"
|
||||
#include <tf/transform_listener.h>
|
||||
|
||||
// TEST CASES
|
||||
TEST_F(SteerDriveControllerTest, testNoOdomFrame)
|
||||
{
|
||||
// wait for ROS
|
||||
while(!isControllerAlive())
|
||||
{
|
||||
ros::Duration(0.1).sleep();
|
||||
}
|
||||
// set up tf listener
|
||||
tf::TransformListener listener;
|
||||
ros::Duration(2.0).sleep();
|
||||
// check the odom frame doesn't exist
|
||||
EXPECT_FALSE(listener.frameExists("odom"));
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
ros::init(argc, argv, "steer_drive_controller_odom_tf_test");
|
||||
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
int ret = RUN_ALL_TESTS();
|
||||
spinner.stop();
|
||||
ros::shutdown();
|
||||
return ret;
|
||||
}
|
||||
@@ -0,0 +1,16 @@
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find steer_drive_controller)/test/common/launch/steer_drive_common.launch" />
|
||||
|
||||
<!-- Load diff drive parameter open loop -->
|
||||
<rosparam command="load" file="$(find steer_drive_controller)/test/steer_drive_controller_open_loop_test/steerbot_open_loop.yaml" />
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="steer_drive_controller_open_loop_test"
|
||||
pkg="steer_drive_controller"
|
||||
type="steer_drive_controller_test"
|
||||
time-limit="80.0">
|
||||
<remap from="cmd_vel" to="steerbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="steerbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,2 @@
|
||||
steerbot_controller:
|
||||
open_loop: true
|
||||
@@ -0,0 +1,19 @@
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find steer_drive_controller)/test/common/launch/steer_drive_common.launch" />
|
||||
|
||||
<!-- Override robot_description with bad urdf -->
|
||||
<param name="robot_description"
|
||||
command="$(find xacro)/xacro '$(find steer_drive_controller)/test/common/urdf/steerbot_sphere_wheels.xacro' --inorder" />
|
||||
<!-- Don't provide the radius parameter, so the controller should break -->
|
||||
<!-- <param name="diffbot_controller/wheel_radius" value="0.11"/> -->
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="steer_drive_controller_wheel_radius_param_fail_test"
|
||||
pkg="steer_drive_controller"
|
||||
type="steer_drive_controller_fail_test"
|
||||
time-limit="10.0">
|
||||
<remap from="cmd_vel" to="steerbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="steerbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,19 @@
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find steer_drive_controller)/test/common/launch/steer_drive_common.launch" />
|
||||
|
||||
<!-- Override robot_description with sphere wheel urdf -->
|
||||
<param name="robot_description"
|
||||
command="$(find xacro)/xacro '$(find steer_drive_controller)/test/common/urdf/steerbot_sphere_wheels.xacro' --inorder" />
|
||||
<!-- Provide the radius, since the bot's wheels are spheres, not cylinders -->
|
||||
<param name="steerbot_controller/wheel_radius" value="0.11"/>
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="steer_drive_controller_wheel_radius_param_test"
|
||||
pkg="steer_drive_controller"
|
||||
type="steer_drive_controller_test"
|
||||
time-limit="80.0">
|
||||
<remap from="cmd_vel" to="steerbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="steerbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,21 @@
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find steer_drive_controller)/test/common/launch/steer_drive_common.launch" />
|
||||
|
||||
<!-- Override robot_description with spere urdf -->
|
||||
<param name="robot_description"
|
||||
command="$(find xacro)/xacro '$(find steer_drive_controller)/test/common/urdf/steerbot_sphere_wheels.xacro' --inorder" />
|
||||
<!-- Provide the radius, since the bot's wheels are spheres, not cylinders -->
|
||||
<param name="steerbot_controller/wheel_radius" value="0.11"/>
|
||||
<!-- Provide the wheel separation -->
|
||||
<param name="steerbot_controller/wheel_separation_h" value="0.4"/>
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="steer_drive_controller_wheel_separation_param_test"
|
||||
pkg="steer_drive_controller"
|
||||
type="steer_drive_controller_test"
|
||||
time-limit="80.0">
|
||||
<remap from="cmd_vel" to="steerbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="steerbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,13 @@
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find steer_drive_controller)/test/common/launch/steer_drive_common.launch" />
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="steer_drive_controller_test"
|
||||
pkg="steer_drive_controller"
|
||||
type="steer_drive_controller_test"
|
||||
time-limit="80.0">
|
||||
<remap from="cmd_vel" to="steerbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="steerbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,158 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// Copyright (C) 2013, PAL Robotics S.L.
|
||||
//
|
||||
// Redistribution and use in source and binary forms, with or without
|
||||
// modification, are permitted provided that the following conditions are met:
|
||||
// * Redistributions of source code must retain the above copyright notice,
|
||||
// this list of conditions and the following disclaimer.
|
||||
// * Redistributions in binary form must reproduce the above copyright
|
||||
// notice, this list of conditions and the following disclaimer in the
|
||||
// documentation and/or other materials provided with the distribution.
|
||||
// * Neither the name of PAL Robotics, Inc. nor the names of its
|
||||
// contributors may be used to endorse or promote products derived from
|
||||
// this software without specific prior written permission.
|
||||
//
|
||||
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
// POSSIBILITY OF SUCH DAMAGE.
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/// \author Bence Magyar
|
||||
|
||||
#include "../common/include/test_common.h"
|
||||
#include <tf/transform_listener.h>
|
||||
|
||||
// TEST CASES
|
||||
TEST_F(SteerDriveControllerTest, testForward)
|
||||
{
|
||||
// wait for ROS
|
||||
while(!isControllerAlive())
|
||||
{
|
||||
ros::Duration(0.1).sleep();
|
||||
}
|
||||
// zero everything before test
|
||||
geometry_msgs::Twist cmd_vel;
|
||||
cmd_vel.linear.x = 0.0;
|
||||
cmd_vel.angular.z = 0.0;
|
||||
publish(cmd_vel);
|
||||
ros::Duration(0.1).sleep();
|
||||
// get initial odom
|
||||
nav_msgs::Odometry old_odom = getLastOdom();
|
||||
// send a velocity command of 0.1 m/s
|
||||
cmd_vel.linear.x = 0.1;
|
||||
publish(cmd_vel);
|
||||
// wait for 10s
|
||||
ros::Duration(10.0).sleep();
|
||||
|
||||
nav_msgs::Odometry new_odom = getLastOdom();
|
||||
|
||||
// check if the robot traveled 1 meter in XY plane, changes in z should be ~~0
|
||||
const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x;
|
||||
const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y;
|
||||
const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z;
|
||||
EXPECT_NEAR(sqrt(dx*dx + dy*dy), 1.0, POSITION_TOLERANCE);
|
||||
EXPECT_LT(fabs(dz), EPS);
|
||||
|
||||
// convert to rpy and test that way
|
||||
double roll_old, pitch_old, yaw_old;
|
||||
double roll_new, pitch_new, yaw_new;
|
||||
tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old);
|
||||
tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new);
|
||||
EXPECT_LT(fabs(roll_new - roll_old), EPS);
|
||||
EXPECT_LT(fabs(pitch_new - pitch_old), EPS);
|
||||
EXPECT_LT(fabs(yaw_new - yaw_old), EPS);
|
||||
EXPECT_NEAR(fabs(new_odom.twist.twist.linear.x), cmd_vel.linear.x, EPS);
|
||||
EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS);
|
||||
EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS);
|
||||
|
||||
EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS);
|
||||
EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS);
|
||||
EXPECT_LT(fabs(new_odom.twist.twist.angular.z), EPS);
|
||||
}
|
||||
|
||||
TEST_F(SteerDriveControllerTest, testTurn)
|
||||
{
|
||||
// wait for ROS
|
||||
while(!isControllerAlive())
|
||||
{
|
||||
ros::Duration(0.1).sleep();
|
||||
}
|
||||
// zero everything before test
|
||||
geometry_msgs::Twist cmd_vel;
|
||||
cmd_vel.linear.x = 0.0;
|
||||
cmd_vel.angular.z = 0.0;
|
||||
publish(cmd_vel);
|
||||
ros::Duration(0.1).sleep();
|
||||
// get initial odom
|
||||
nav_msgs::Odometry old_odom = getLastOdom();
|
||||
// send a velocity command
|
||||
cmd_vel.angular.z = M_PI/10.0;
|
||||
// send linear command too
|
||||
// because sending only angular command doesn't actuate wheels for steer drive mechanism
|
||||
cmd_vel.linear.x = 0.1;
|
||||
publish(cmd_vel);
|
||||
// wait for 10s
|
||||
ros::Duration(10.0).sleep();
|
||||
|
||||
nav_msgs::Odometry new_odom = getLastOdom();
|
||||
|
||||
// check if the robot rotated PI around z, changes in x should be ~~0 and in y should be y_answer
|
||||
double x_answer = 0.0;
|
||||
double y_answer = 2.0 * cmd_vel.linear.x / cmd_vel.angular.z; // R = v/w, D = 2R
|
||||
EXPECT_NEAR(fabs(new_odom.pose.pose.position.x - old_odom.pose.pose.position.x), x_answer, POSITION_TOLERANCE);
|
||||
EXPECT_NEAR(fabs(new_odom.pose.pose.position.y - old_odom.pose.pose.position.y), y_answer, POSITION_TOLERANCE);
|
||||
EXPECT_LT(fabs(new_odom.pose.pose.position.z - old_odom.pose.pose.position.z), EPS);
|
||||
|
||||
// convert to rpy and test that way
|
||||
double roll_old, pitch_old, yaw_old;
|
||||
double roll_new, pitch_new, yaw_new;
|
||||
tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old);
|
||||
tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new);
|
||||
EXPECT_LT(fabs(roll_new - roll_old), EPS);
|
||||
EXPECT_LT(fabs(pitch_new - pitch_old), EPS);
|
||||
EXPECT_NEAR(fabs(yaw_new - yaw_old), M_PI, ORIENTATION_TOLERANCE);
|
||||
|
||||
EXPECT_NEAR(new_odom.twist.twist.linear.x, 0.1, VELOCITY_TOLERANCE);
|
||||
EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS);
|
||||
EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS);
|
||||
|
||||
EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS);
|
||||
EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS);
|
||||
EXPECT_NEAR(fabs(new_odom.twist.twist.angular.z), M_PI/10.0, EPS);
|
||||
}
|
||||
|
||||
TEST_F(SteerDriveControllerTest, testOdomFrame)
|
||||
{
|
||||
// wait for ROS
|
||||
while(!isControllerAlive())
|
||||
{
|
||||
ros::Duration(0.1).sleep();
|
||||
}
|
||||
// set up tf listener
|
||||
tf::TransformListener listener;
|
||||
ros::Duration(2.0).sleep();
|
||||
// check the odom frame exist
|
||||
EXPECT_TRUE(listener.frameExists("odom"));
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
ros::init(argc, argv, "steer_drive_controller_test");
|
||||
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
//ros::Duration(0.5).sleep();
|
||||
int ret = RUN_ALL_TESTS();
|
||||
spinner.stop();
|
||||
ros::shutdown();
|
||||
return ret;
|
||||
}
|
||||
@@ -0,0 +1,16 @@
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find steer_drive_controller)/test/common/launch/steer_drive_common.launch" />
|
||||
|
||||
<!-- Load steer-drive cmd_vel_timeout -->
|
||||
<rosparam command="load" file="$(find steer_drive_controller)/test/steer_drive_controller_timeout_test/steerbot_timeout.yaml" />
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="steer_drive_controller_timeout_test"
|
||||
pkg="steer_drive_controller"
|
||||
type="steer_drive_controller_timeout_test"
|
||||
time-limit="20.0">
|
||||
<remap from="cmd_vel" to="steerbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="steerbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,72 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// Copyright (C) 2013, PAL Robotics S.L.
|
||||
//
|
||||
// Redistribution and use in source and binary forms, with or without
|
||||
// modification, are permitted provided that the following conditions are met:
|
||||
// * Redistributions of source code must retain the above copyright notice,
|
||||
// this list of conditions and the following disclaimer.
|
||||
// * Redistributions in binary form must reproduce the above copyright
|
||||
// notice, this list of conditions and the following disclaimer in the
|
||||
// documentation and/or other materials provided with the distribution.
|
||||
// * Neither the name of PAL Robotics, Inc. nor the names of its
|
||||
// contributors may be used to endorse or promote products derived from
|
||||
// this software without specific prior written permission.
|
||||
//
|
||||
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
// POSSIBILITY OF SUCH DAMAGE.
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/// \author Paul Mathieu
|
||||
|
||||
#include "../common/include/test_common.h"
|
||||
|
||||
// TEST CASES
|
||||
TEST_F(SteerDriveControllerTest, testTimeout)
|
||||
{
|
||||
// wait for ROS
|
||||
while(!isControllerAlive())
|
||||
{
|
||||
ros::Duration(0.1).sleep();
|
||||
}
|
||||
// zero everything before test
|
||||
geometry_msgs::Twist cmd_vel;
|
||||
cmd_vel.linear.x = 0.0;
|
||||
cmd_vel.angular.z = 0.0;
|
||||
publish(cmd_vel);
|
||||
// give some time to the controller to react to the command
|
||||
ros::Duration(0.1).sleep();
|
||||
// get initial odom
|
||||
nav_msgs::Odometry old_odom = getLastOdom();
|
||||
// send a velocity command of 1 m/s
|
||||
cmd_vel.linear.x = 1.0;
|
||||
publish(cmd_vel);
|
||||
// wait a bit
|
||||
ros::Duration(3.0).sleep();
|
||||
|
||||
nav_msgs::Odometry new_odom = getLastOdom();
|
||||
|
||||
// check if the robot has stopped after 0.5s, thus covering less than 0.5s*1.0m.s-1 + some (big) tolerance
|
||||
EXPECT_LT(fabs(new_odom.pose.pose.position.x - old_odom.pose.pose.position.x), 0.8);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
ros::init(argc, argv, "steer_drive_controller_timeout_test");
|
||||
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
int ret = RUN_ALL_TESTS();
|
||||
spinner.stop();
|
||||
ros::shutdown();
|
||||
return ret;
|
||||
}
|
||||
@@ -0,0 +1,2 @@
|
||||
steerbot_controller:
|
||||
cmd_vel_timeout: 0.5
|
||||
Reference in New Issue
Block a user