git commit -m "first commit for v2"
This commit is contained in:
237
Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/CHANGELOG.rst
Executable file
237
Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/CHANGELOG.rst
Executable file
@@ -0,0 +1,237 @@
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
Changelog for package diff_drive_controller
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
0.21.2 (2023-09-03)
|
||||
-------------------
|
||||
|
||||
0.21.1 (2023-01-31)
|
||||
-------------------
|
||||
* Don't hardcode plugin library path
|
||||
* Contributors: Jochen Sprickerhof
|
||||
|
||||
0.21.0 (2022-10-21)
|
||||
-------------------
|
||||
* std::bind and placeholders instead of boost
|
||||
* use boost::placeholders::_1/_2 in remaining instances, include boost/bind/bind.hpp instead of boost/bind.hpp, eliminated unnecessary bind.hpp include
|
||||
* Contributors: Lucas Walter
|
||||
|
||||
0.20.0 (2022-05-06)
|
||||
-------------------
|
||||
* Drop old C++ standard
|
||||
* Use new boost bind placeholders
|
||||
* Add <?xml version=1.0?> to every .launch and .test file
|
||||
* Contributors: Jochen Sprickerhof, Lucas Walter
|
||||
|
||||
0.19.0 (2021-06-13)
|
||||
-------------------
|
||||
* fix NaN bug
|
||||
* fix test to expose NaN bug
|
||||
* Wait long enough for accumulator to be cleared
|
||||
* Add test for `#532 <https://github.com/ros-controls/ros_controllers/issues/532>`_
|
||||
Close `#540 <https://github.com/ros-controls/ros_controllers/issues/540>`_
|
||||
* Contributors: Caio Amaral, Matt Reynolds, Melvin Wang
|
||||
|
||||
0.18.1 (2020-12-03)
|
||||
-------------------
|
||||
* Fix null pointer checks in diff_drive_controller
|
||||
* Use version-agnostic FindBoost for headers
|
||||
* Contributors: David V. Lu, Matt Reynolds
|
||||
|
||||
0.18.0 (2020-10-11)
|
||||
-------------------
|
||||
* Fix dependency on Boost
|
||||
* Apply consistent format to CMakeLists.txt
|
||||
* Update package.xml to format 3
|
||||
* Clean dependencies of diff_drive_controller package
|
||||
* Apply waitForController method to all diff_drive_controller tests
|
||||
* Move odom_pub setup to the end to allow consistent isControllerAlive check
|
||||
* Contributors: Mateus Amarante
|
||||
|
||||
0.17.0 (2020-05-12)
|
||||
-------------------
|
||||
|
||||
0.16.1 (2020-04-27)
|
||||
-------------------
|
||||
|
||||
0.16.0 (2020-04-16)
|
||||
-------------------
|
||||
* Fix warning dynamic_reconfigure
|
||||
* Bump CMake version to prevent CMP0048
|
||||
* Add missing header guards
|
||||
* Replace header guard with #pragma once
|
||||
* Prefix every xacro tag with 'xacro:'
|
||||
* Modernize xacro
|
||||
- Remove '--inorder'
|
||||
- Use 'xacro' over 'xacro.py'
|
||||
* switch implementation of read and write methods of Diffbot class
|
||||
* Refactor nan test
|
||||
EXPECT_NE(x, bool) -> EXPECT_TRUE/FALSE(x)
|
||||
EXPECT_EQ(x, double) -> EXPECT_DOUBLE_EQ(x, double)
|
||||
+ clang default reformat
|
||||
* Check for nan cmd_vel
|
||||
* Contributors: Anas Abou Allaban, Bence Magyar, Franz, Matt Reynolds, Raffaello Bonghi
|
||||
|
||||
0.15.1 (2020-03-09)
|
||||
-------------------
|
||||
* Use nullptr
|
||||
* add missing pluginlib deps.
|
||||
* Update null link pointer error message
|
||||
* Revert cmake include catkin_INCLUDE_DIRS as system
|
||||
* Use C++11 `std::this_thread::sleep_for`.
|
||||
* Contributors: Bence Magyar, Enrique Fernandez Perdomo, Matt Reynolds, Sean Yen
|
||||
|
||||
0.15.0 (2019-03-26)
|
||||
-------------------
|
||||
* Default all controller builds to C++14
|
||||
* boost::chrono -> std::chrono
|
||||
* boost::assign -> C++ initializer list
|
||||
* boost::shared_ptr -> std::shared_ptr
|
||||
* Using left/right multiplies for desired vel
|
||||
* diff-drive publish joint trajectory controller state
|
||||
* fix install destination for libraries (`#403 <https://github.com/ros-controls/ros_controllers/issues/403>`_)
|
||||
* Contributors: Bence Magyar, Gennaro Raiola, James Xu, Jeremie Deray, Jordan Palacios
|
||||
|
||||
0.14.3 (2019-02-09)
|
||||
-------------------
|
||||
* use operators instead of aliases
|
||||
* Fix typo descripion -> description
|
||||
* Contributors: Daniel Ingram, James Xu
|
||||
|
||||
0.14.2 (2018-10-23)
|
||||
-------------------
|
||||
|
||||
0.14.1 (2018-06-26)
|
||||
-------------------
|
||||
* Added 'multiplier' in DynamicParams ostream and changed boolean printing to 'enabled/disabled'
|
||||
* isPublishngCmdVelOut to check getNumPublisheres until timeout
|
||||
* Contributors: Kei Okada, Martin Ganeff
|
||||
|
||||
0.14.0 (2018-04-27)
|
||||
-------------------
|
||||
* add dynamic_reconf to diff_drive_controller
|
||||
* migrate to new pluginlib headers
|
||||
* per wheel radius multiplier
|
||||
* fix xacro macro warning
|
||||
* [DiffDrive] Fix time-sensitive tests of diff_drive_controller
|
||||
* separate include_directories as SYSTEM to avoid unrelated compilation warnings
|
||||
* Contributors: Jeremie Deray, Mathias Lüdtke
|
||||
|
||||
0.13.2 (2017-12-23)
|
||||
-------------------
|
||||
|
||||
0.13.1 (2017-11-06)
|
||||
-------------------
|
||||
|
||||
0.13.0 (2017-08-10)
|
||||
-------------------
|
||||
* Add test for allow_multiple_cmd_vel_publishers param
|
||||
* add check for multiple publishers on cmd_vel
|
||||
* Added tests for the odom_frame_id parameter.
|
||||
* Parameterized diff_drive_controller's odom_frame_id
|
||||
* Publish executed velocity if publish_cmd
|
||||
* refactor to remove code duplication
|
||||
* fixup pointer type for new convention
|
||||
* Allow diff_drive_controller to use spheres as well as cylinders for wheel collision geometry. Cylinders are not well behaved on Gazebo/ODE heightfields, using spheres works around the issue.
|
||||
* Contributors: Bence Magyar, Eric Tappan, Jeremie Deray, Karsten Knese, Tully Foote, mallanmba, tappan-at-git
|
||||
|
||||
0.12.3 (2017-04-23)
|
||||
-------------------
|
||||
|
||||
0.12.2 (2017-04-21)
|
||||
-------------------
|
||||
|
||||
0.12.1 (2017-03-08)
|
||||
-------------------
|
||||
* Add exporting include dirs
|
||||
* Contributors: Bence Magyar
|
||||
|
||||
0.12.0 (2017-02-15)
|
||||
-------------------
|
||||
* Fix most catkin lint issues
|
||||
* Change for format2
|
||||
* Add Enrique and Bence to maintainers
|
||||
* Add urdf compatibility header
|
||||
* Add --inorder to xacro calls
|
||||
* Add missing xacro tags
|
||||
* Use xacro instead of xacro.py
|
||||
* Disable angular jerk limit test
|
||||
* Replace boost::shared_ptr<urdf::XY> with urdf::XYConstSharedPtr when exists
|
||||
* Contributors: Bence Magyar
|
||||
|
||||
0.11.2 (2016-08-16)
|
||||
-------------------
|
||||
|
||||
0.11.1 (2016-05-23)
|
||||
-------------------
|
||||
|
||||
0.11.0 (2016-05-03)
|
||||
-------------------
|
||||
|
||||
0.10.0 (2015-11-20)
|
||||
-------------------
|
||||
* 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 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
|
||||
* Contributors: Adolfo Rodriguez Tsouroukdissian, Enrique Fernandez, Paul Mathieu
|
||||
|
||||
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.
|
||||
216
Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/CMakeLists.txt
Executable file
216
Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/CMakeLists.txt
Executable file
@@ -0,0 +1,216 @@
|
||||
cmake_minimum_required(VERSION 3.0.2)
|
||||
project(diff_drive_controller)
|
||||
|
||||
# Load catkin and all dependencies required for this package
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
controller_interface
|
||||
control_msgs
|
||||
dynamic_reconfigure
|
||||
geometry_msgs
|
||||
hardware_interface
|
||||
nav_msgs
|
||||
pluginlib
|
||||
realtime_tools
|
||||
tf
|
||||
urdf
|
||||
)
|
||||
|
||||
find_package(Boost REQUIRED)
|
||||
|
||||
# Declare a catkin package
|
||||
generate_dynamic_reconfigure_options(cfg/DiffDriveController.cfg)
|
||||
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
CATKIN_DEPENDS
|
||||
controller_interface
|
||||
control_msgs
|
||||
dynamic_reconfigure
|
||||
geometry_msgs
|
||||
hardware_interface
|
||||
nav_msgs
|
||||
realtime_tools
|
||||
tf
|
||||
LIBRARIES ${PROJECT_NAME}
|
||||
DEPENDS Boost
|
||||
)
|
||||
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
# Specify header include paths
|
||||
include_directories(
|
||||
include ${catkin_INCLUDE_DIRS}
|
||||
include ${Boost_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
add_library(${PROJECT_NAME}
|
||||
src/diff_drive_controller.cpp
|
||||
src/odometry.cpp
|
||||
src/speed_limiter.cpp
|
||||
)
|
||||
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
|
||||
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
|
||||
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
if (CATKIN_ENABLE_TESTING)
|
||||
find_package(controller_manager REQUIRED)
|
||||
find_package(rosgraph_msgs REQUIRED)
|
||||
find_package(std_srvs REQUIRED)
|
||||
find_package(rostest REQUIRED)
|
||||
|
||||
include_directories(
|
||||
${controller_manager_INCLUDE_DIRS}
|
||||
${rosgraph_msgs_INCLUDE_DIRS}
|
||||
${std_srvs_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
set(test_LIBRARIES
|
||||
${controller_manager_LIBRARIES}
|
||||
${rosgraph_msgs_LIBRARIES}
|
||||
${std_srvs_LIBRARIES}
|
||||
)
|
||||
|
||||
add_executable(diffbot test/diffbot.cpp)
|
||||
target_link_libraries(diffbot ${catkin_LIBRARIES} ${test_LIBRARIES})
|
||||
|
||||
add_executable(skidsteerbot test/skidsteerbot.cpp)
|
||||
target_link_libraries(skidsteerbot ${catkin_LIBRARIES} ${test_LIBRARIES})
|
||||
|
||||
add_dependencies(tests diffbot skidsteerbot)
|
||||
|
||||
add_rostest_gtest(diff_drive_test
|
||||
test/diff_drive_controller.test
|
||||
test/diff_drive_test.cpp
|
||||
)
|
||||
target_link_libraries(diff_drive_test ${catkin_LIBRARIES} ${test_LIBRARIES})
|
||||
|
||||
add_rostest_gtest(diff_drive_nan_test
|
||||
test/diff_drive_controller_nan.test
|
||||
test/diff_drive_nan_test.cpp
|
||||
)
|
||||
target_link_libraries(diff_drive_nan_test ${catkin_LIBRARIES} ${test_LIBRARIES})
|
||||
|
||||
add_rostest_gtest(diff_drive_limits_test
|
||||
test/diff_drive_controller_limits.test
|
||||
test/diff_drive_limits_test.cpp
|
||||
)
|
||||
target_link_libraries(diff_drive_limits_test ${catkin_LIBRARIES} ${test_LIBRARIES})
|
||||
|
||||
add_rostest_gtest(diff_drive_timeout_test
|
||||
test/diff_drive_timeout.test
|
||||
test/diff_drive_timeout_test.cpp
|
||||
)
|
||||
target_link_libraries(diff_drive_timeout_test ${catkin_LIBRARIES} ${test_LIBRARIES})
|
||||
|
||||
add_rostest(test/diff_drive_multipliers.test)
|
||||
|
||||
add_rostest(test/diff_drive_left_right_multipliers.test)
|
||||
|
||||
add_rostest_gtest(diff_drive_fail_test
|
||||
test/diff_drive_wrong.test
|
||||
test/diff_drive_fail_test.cpp
|
||||
)
|
||||
target_link_libraries(diff_drive_fail_test ${catkin_LIBRARIES} ${test_LIBRARIES})
|
||||
|
||||
add_rostest_gtest(diff_drive_odom_tf_test
|
||||
test/diff_drive_odom_tf.test
|
||||
test/diff_drive_odom_tf_test.cpp
|
||||
)
|
||||
target_link_libraries(diff_drive_odom_tf_test ${catkin_LIBRARIES} ${test_LIBRARIES})
|
||||
|
||||
add_rostest_gtest(diff_drive_default_cmd_vel_out_test
|
||||
test/diff_drive_default_cmd_vel_out.test
|
||||
test/diff_drive_default_cmd_vel_out_test.cpp
|
||||
)
|
||||
target_link_libraries(diff_drive_default_cmd_vel_out_test ${catkin_LIBRARIES} ${test_LIBRARIES})
|
||||
|
||||
add_rostest_gtest(diff_drive_pub_cmd_vel_out_test
|
||||
test/diff_drive_pub_cmd_vel_out.test
|
||||
test/diff_drive_pub_cmd_vel_out_test.cpp
|
||||
)
|
||||
target_link_libraries(diff_drive_pub_cmd_vel_out_test ${catkin_LIBRARIES} ${test_LIBRARIES})
|
||||
|
||||
add_rostest_gtest(diff_drive_default_wheel_joint_controller_state_test
|
||||
test/diff_drive_default_wheel_joint_controller_state.test
|
||||
test/diff_drive_default_wheel_joint_controller_state_test.cpp
|
||||
)
|
||||
target_link_libraries(diff_drive_default_wheel_joint_controller_state_test ${catkin_LIBRARIES} ${test_LIBRARIES})
|
||||
|
||||
add_rostest_gtest(diff_drive_publish_wheel_joint_controller_state_test
|
||||
test/diff_drive_publish_wheel_joint_controller_state.test
|
||||
test/diff_drive_publish_wheel_joint_controller_state_test.cpp
|
||||
)
|
||||
target_link_libraries(diff_drive_publish_wheel_joint_controller_state_test ${catkin_LIBRARIES} ${test_LIBRARIES})
|
||||
|
||||
add_rostest_gtest(diff_drive_default_odom_frame_test
|
||||
test/diff_drive_default_odom_frame.test
|
||||
test/diff_drive_default_odom_frame_test.cpp
|
||||
)
|
||||
target_link_libraries(diff_drive_default_odom_frame_test ${catkin_LIBRARIES} ${test_LIBRARIES})
|
||||
|
||||
add_rostest_gtest(diff_drive_odom_frame_test
|
||||
test/diff_drive_odom_frame.test
|
||||
test/diff_drive_odom_frame_test.cpp
|
||||
)
|
||||
target_link_libraries(diff_drive_odom_frame_test ${catkin_LIBRARIES} ${test_LIBRARIES})
|
||||
|
||||
add_rostest_gtest(diff_drive_multiple_cmd_vel_publishers_test
|
||||
test/diff_drive_multiple_cmd_vel_publishers.test
|
||||
test/diff_drive_multiple_cmd_vel_publishers_test.cpp
|
||||
)
|
||||
target_link_libraries(diff_drive_multiple_cmd_vel_publishers_test ${catkin_LIBRARIES} ${test_LIBRARIES})
|
||||
|
||||
add_rostest(test/diff_drive_open_loop.test)
|
||||
|
||||
add_rostest(test/skid_steer_controller.test)
|
||||
|
||||
add_rostest(test/skid_steer_no_wheels.test)
|
||||
|
||||
add_rostest(test/diff_drive_radius_sphere.test)
|
||||
|
||||
add_rostest(test/diff_drive_radius_param.test)
|
||||
|
||||
add_rostest(test/diff_drive_radius_param_fail.test)
|
||||
|
||||
add_rostest(test/diff_drive_separation_param.test)
|
||||
|
||||
add_rostest(test/diff_drive_bad_urdf.test)
|
||||
|
||||
add_rostest(test/diff_drive_get_wheel_radius_fail.test)
|
||||
|
||||
add_rostest_gtest(diff_drive_dyn_reconf_test
|
||||
test/diff_drive_dyn_reconf.test
|
||||
test/diff_drive_dyn_reconf_test.cpp
|
||||
)
|
||||
target_link_libraries(diff_drive_dyn_reconf_test ${catkin_LIBRARIES} ${test_LIBRARIES})
|
||||
|
||||
endif()
|
||||
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
# Install headers
|
||||
install(DIRECTORY include/${PROJECT_NAME}/
|
||||
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
)
|
||||
|
||||
# Install library
|
||||
install(TARGETS ${PROJECT_NAME}
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
|
||||
)
|
||||
|
||||
# Install plugins
|
||||
install(FILES diff_drive_controller_plugins.xml
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
)
|
||||
@@ -0,0 +1,5 @@
|
||||
## Diff Drive Controller ##
|
||||
|
||||
Controller for a differential drive mobile base.
|
||||
|
||||
Detailed user documentation can be found in the controller's [ROS wiki page](http://wiki.ros.org/diff_drive_controller).
|
||||
@@ -0,0 +1,18 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
PACKAGE = 'diff_drive_controller'
|
||||
|
||||
from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t
|
||||
|
||||
gen = ParameterGenerator()
|
||||
|
||||
# Kinematic parameters related
|
||||
gen.add("left_wheel_radius_multiplier", double_t, 0, "Left wheel radius multiplier.", 1.0, 0.5, 1.5)
|
||||
gen.add("right_wheel_radius_multiplier", double_t, 0, "Right wheel radius multiplier.", 1.0, 0.5, 1.5)
|
||||
gen.add("wheel_separation_multiplier", double_t, 0, "Wheel separation multiplier.", 1.0, 0.5, 1.5)
|
||||
|
||||
# Publication related
|
||||
gen.add("publish_rate", double_t, 0, "Publish rate of odom.", 50.0, 0.0, 2000.0)
|
||||
gen.add("enable_odom_tf", bool_t, 0, "Publish odom frame to tf.", True)
|
||||
|
||||
exit(gen.generate(PACKAGE, "diff_drive_controller", "DiffDriveController"))
|
||||
@@ -0,0 +1,9 @@
|
||||
<library path="libdiff_drive_controller">
|
||||
|
||||
<class name="diff_drive_controller/DiffDriveController" type="diff_drive_controller::DiffDriveController" base_class_type="controller_interface::ControllerBase">
|
||||
<description>
|
||||
The DiffDriveController tracks velocity commands. It expects 2 VelocityJointInterface type of hardware interfaces.
|
||||
</description>
|
||||
</class>
|
||||
|
||||
</library>
|
||||
@@ -0,0 +1,310 @@
|
||||
/*********************************************************************
|
||||
* 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, Enrique Fernández
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include <control_msgs/JointTrajectoryControllerState.h>
|
||||
#include <controller_interface/controller.h>
|
||||
#include <diff_drive_controller/DiffDriveControllerConfig.h>
|
||||
#include <diff_drive_controller/odometry.h>
|
||||
#include <diff_drive_controller/speed_limiter.h>
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include <geometry_msgs/TwistStamped.h>
|
||||
#include <hardware_interface/joint_command_interface.h>
|
||||
#include <memory>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <realtime_tools/realtime_buffer.h>
|
||||
#include <realtime_tools/realtime_publisher.h>
|
||||
#include <tf/tfMessage.h>
|
||||
|
||||
namespace diff_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 or sphere in the urdf
|
||||
* - a wheel joint frame center's vertical projection on the floor must lie within the contact patch
|
||||
*/
|
||||
class DiffDriveController
|
||||
: public controller_interface::Controller<hardware_interface::VelocityJointInterface>
|
||||
{
|
||||
public:
|
||||
DiffDriveController();
|
||||
|
||||
/**
|
||||
* \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,
|
||||
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:
|
||||
std::vector<hardware_interface::JointHandle> left_wheel_joints_;
|
||||
std::vector<hardware_interface::JointHandle> right_wheel_joints_;
|
||||
|
||||
// Previous time
|
||||
ros::Time time_previous_;
|
||||
|
||||
/// Previous velocities from the encoders:
|
||||
std::vector<double> vel_left_previous_;
|
||||
std::vector<double> vel_right_previous_;
|
||||
|
||||
/// Previous velocities from the encoders:
|
||||
double vel_left_desired_previous_;
|
||||
double vel_right_desired_previous_;
|
||||
|
||||
/// 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_;
|
||||
|
||||
/// Publish executed commands
|
||||
std::shared_ptr<realtime_tools::RealtimePublisher<geometry_msgs::TwistStamped> > cmd_vel_pub_;
|
||||
|
||||
/// Odometry related:
|
||||
std::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::Odometry> > odom_pub_;
|
||||
std::shared_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > tf_odom_pub_;
|
||||
Odometry odometry_;
|
||||
|
||||
/// Controller state publisher
|
||||
std::shared_ptr<realtime_tools::RealtimePublisher<control_msgs::JointTrajectoryControllerState> > controller_state_pub_;
|
||||
|
||||
/// Wheel separation, wrt the midpoint of the wheel width:
|
||||
double wheel_separation_;
|
||||
|
||||
/// 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_multiplier_;
|
||||
double left_wheel_radius_multiplier_;
|
||||
double right_wheel_radius_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_;
|
||||
|
||||
/// Speed limiters:
|
||||
Commands last1_cmd_;
|
||||
Commands last0_cmd_;
|
||||
SpeedLimiter limiter_lin_;
|
||||
SpeedLimiter limiter_ang_;
|
||||
|
||||
/// Publish limited velocity:
|
||||
bool publish_cmd_;
|
||||
|
||||
/// Publish wheel data:
|
||||
bool publish_wheel_joint_controller_state_;
|
||||
|
||||
// A struct to hold dynamic parameters
|
||||
// set from dynamic_reconfigure server
|
||||
struct DynamicParams
|
||||
{
|
||||
bool update;
|
||||
|
||||
double left_wheel_radius_multiplier;
|
||||
double right_wheel_radius_multiplier;
|
||||
double wheel_separation_multiplier;
|
||||
|
||||
bool publish_cmd;
|
||||
|
||||
double publish_rate;
|
||||
bool enable_odom_tf;
|
||||
|
||||
DynamicParams()
|
||||
: left_wheel_radius_multiplier(1.0)
|
||||
, right_wheel_radius_multiplier(1.0)
|
||||
, wheel_separation_multiplier(1.0)
|
||||
, publish_cmd(false)
|
||||
, publish_rate(50)
|
||||
, enable_odom_tf(true)
|
||||
{}
|
||||
|
||||
friend std::ostream& operator<<(std::ostream& os, const DynamicParams& params)
|
||||
{
|
||||
os << "DynamicParams:\n"
|
||||
//
|
||||
<< "\tOdometry parameters:\n"
|
||||
<< "\t\tleft wheel radius multiplier: " << params.left_wheel_radius_multiplier << "\n"
|
||||
<< "\t\tright wheel radius multiplier: " << params.right_wheel_radius_multiplier << "\n"
|
||||
<< "\t\twheel separation multiplier: " << params.wheel_separation_multiplier << "\n"
|
||||
//
|
||||
<< "\tPublication parameters:\n"
|
||||
<< "\t\tPublish executed velocity command: " << (params.publish_cmd?"enabled":"disabled") << "\n"
|
||||
<< "\t\tPublication rate: " << params.publish_rate << "\n"
|
||||
<< "\t\tPublish frame odom on tf: " << (params.enable_odom_tf?"enabled":"disabled");
|
||||
|
||||
return os;
|
||||
}
|
||||
};
|
||||
|
||||
realtime_tools::RealtimeBuffer<DynamicParams> dynamic_params_;
|
||||
|
||||
/// Dynamic Reconfigure server
|
||||
typedef dynamic_reconfigure::Server<DiffDriveControllerConfig> ReconfigureServer;
|
||||
std::shared_ptr<ReconfigureServer> dyn_reconf_server_;
|
||||
boost::recursive_mutex dyn_reconf_server_mutex_;
|
||||
|
||||
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 Get the wheel names from a wheel param
|
||||
* \param [in] controller_nh Controller node handler
|
||||
* \param [in] wheel_param Param name
|
||||
* \param [out] wheel_names Vector with the whel names
|
||||
* \return true if the wheel_param is available and the wheel_names are
|
||||
* retrieved successfully from the param server; false otherwise
|
||||
*/
|
||||
bool getWheelNames(ros::NodeHandle& controller_nh,
|
||||
const std::string& wheel_param,
|
||||
std::vector<std::string>& wheel_names);
|
||||
|
||||
/**
|
||||
* \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& left_wheel_name,
|
||||
const std::string& right_wheel_name,
|
||||
bool lookup_wheel_separation,
|
||||
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);
|
||||
|
||||
/**
|
||||
* \brief Callback for dynamic_reconfigure server
|
||||
* \param config The config set from dynamic_reconfigure server
|
||||
* \param level not used at this time.
|
||||
* \see dyn_reconf_server_
|
||||
*/
|
||||
void reconfCallback(DiffDriveControllerConfig& config, uint32_t /*level*/);
|
||||
|
||||
/**
|
||||
* \brief Update the dynamic parameters in the RT loop
|
||||
*/
|
||||
void updateDynamicParams();
|
||||
|
||||
/**
|
||||
* \brief
|
||||
* \param time Current time
|
||||
* \param period Time since the last called to update
|
||||
* \param curr_cmd Current velocity command
|
||||
* \param wheel_separation wheel separation with multiplier
|
||||
* \param left_wheel_radius left wheel radius with multiplier
|
||||
* \param right_wheel_radius right wheel radius with multiplier
|
||||
*/
|
||||
void publishWheelData(const ros::Time& time,
|
||||
const ros::Duration& period,
|
||||
Commands& curr_cmd,
|
||||
double wheel_separation,
|
||||
double left_wheel_radius,
|
||||
double right_wheel_radius);
|
||||
};
|
||||
|
||||
} // namespace diff_drive_controller
|
||||
@@ -0,0 +1,210 @@
|
||||
/*********************************************************************
|
||||
* 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
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
#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 diff_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 left_pos Left wheel position [rad]
|
||||
* \param right_pos Right wheel position [rad]
|
||||
* \param time Current time
|
||||
* \return true if the odometry is actually updated
|
||||
*/
|
||||
bool update(double left_pos, double right_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 Separation between left and right wheels [m]
|
||||
* \param left_wheel_radius Left wheel radius [m]
|
||||
* \param right_wheel_radius Right wheel radius [m]
|
||||
*/
|
||||
void setWheelParams(double wheel_separation, double left_wheel_radius, double right_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_;
|
||||
double left_wheel_radius_;
|
||||
double right_wheel_radius_;
|
||||
|
||||
/// Previou wheel position/state [rad]:
|
||||
double left_wheel_old_pos_;
|
||||
double right_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_;
|
||||
};
|
||||
}
|
||||
@@ -0,0 +1,129 @@
|
||||
/*********************************************************************
|
||||
* 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
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
namespace diff_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
|
||||
51
Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/package.xml
Executable file
51
Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/package.xml
Executable file
@@ -0,0 +1,51 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model
|
||||
href="http://download.ros.org/schema/package_format3.xsd"
|
||||
schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>diff_drive_controller</name>
|
||||
<version>0.21.2</version>
|
||||
<description>Controller for a differential drive mobile base.</description>
|
||||
|
||||
<maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
|
||||
<maintainer email="enrique.fernandez.perdomo@gmail.com">Enrique Fernandez</maintainer>
|
||||
|
||||
<license>BSD</license>
|
||||
|
||||
<url type="website">https://github.com/ros-controls/ros_controllers/wiki</url>
|
||||
<url type="bugtracker">https://github.com/ros-controls/ros_controllers/issues</url>
|
||||
<url type="repository">https://github.com/ros-controls/ros_controllers</url>
|
||||
|
||||
<author email="bence.magyar@pal-robotics.com">Bence Magyar</author>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<depend>controller_interface</depend>
|
||||
<depend>control_msgs</depend>
|
||||
<depend>dynamic_reconfigure</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>hardware_interface</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>realtime_tools</depend>
|
||||
<depend>tf</depend>
|
||||
|
||||
<build_depend>boost</build_depend>
|
||||
<build_depend>pluginlib</build_depend>
|
||||
<build_depend>urdf</build_depend>
|
||||
|
||||
<build_export_depend>boost</build_export_depend>
|
||||
|
||||
<exec_depend>pluginlib</exec_depend>
|
||||
<exec_depend>urdf</exec_depend>
|
||||
|
||||
<test_depend>controller_manager</test_depend>
|
||||
<test_depend>rosgraph_msgs</test_depend>
|
||||
<test_depend>rostest</test_depend>
|
||||
<test_depend>rostopic</test_depend>
|
||||
<test_depend>std_srvs</test_depend>
|
||||
<test_depend>xacro</test_depend>
|
||||
|
||||
<export>
|
||||
<controller_interface plugin="${prefix}/diff_drive_controller_plugins.xml"/>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,840 @@
|
||||
/*********************************************************************
|
||||
* 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, Enrique Fernández
|
||||
*/
|
||||
|
||||
#include <cmath>
|
||||
#include <diff_drive_controller/diff_drive_controller.h>
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
#include <tf/transform_datatypes.h>
|
||||
#include <urdf/urdfdom_compatibility.h>
|
||||
#include <urdf_parser/urdf_parser.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 that a link exists and has a geometry collision.
|
||||
* \param link The link
|
||||
* \return true if the link has a collision element with geometry
|
||||
*/
|
||||
static bool hasCollisionGeometry(const urdf::LinkConstSharedPtr& link)
|
||||
{
|
||||
if (!link)
|
||||
{
|
||||
ROS_ERROR("Link pointer is 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;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/*
|
||||
* \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 (!hasCollisionGeometry(link))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (link->collision->geometry->type != urdf::Geometry::CYLINDER)
|
||||
{
|
||||
ROS_DEBUG_STREAM("Link " << link->name << " does not have cylinder geometry");
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/*
|
||||
* \brief Check if the link is modeled as a sphere
|
||||
* \param link Link
|
||||
* \return true if the link is modeled as a Sphere; false otherwise
|
||||
*/
|
||||
static bool isSphere(const urdf::LinkConstSharedPtr& link)
|
||||
{
|
||||
if (!hasCollisionGeometry(link))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (link->collision->geometry->type != urdf::Geometry::SPHERE)
|
||||
{
|
||||
ROS_DEBUG_STREAM("Link " << link->name << " does not have sphere 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 otherwise
|
||||
*/
|
||||
static bool getWheelRadius(const urdf::LinkConstSharedPtr& wheel_link, double& wheel_radius)
|
||||
{
|
||||
if (isCylinder(wheel_link))
|
||||
{
|
||||
wheel_radius = (static_cast<urdf::Cylinder*>(wheel_link->collision->geometry.get()))->radius;
|
||||
return true;
|
||||
}
|
||||
else if (isSphere(wheel_link))
|
||||
{
|
||||
wheel_radius = (static_cast<urdf::Sphere*>(wheel_link->collision->geometry.get()))->radius;
|
||||
return true;
|
||||
}
|
||||
|
||||
ROS_ERROR_STREAM("Wheel link " << wheel_link->name << " is NOT modeled as a cylinder or sphere!");
|
||||
return false;
|
||||
}
|
||||
|
||||
namespace diff_drive_controller{
|
||||
|
||||
DiffDriveController::DiffDriveController()
|
||||
: open_loop_(false)
|
||||
, command_struct_()
|
||||
, wheel_separation_(0.0)
|
||||
, wheel_radius_(0.0)
|
||||
, wheel_separation_multiplier_(1.0)
|
||||
, left_wheel_radius_multiplier_(1.0)
|
||||
, right_wheel_radius_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)
|
||||
, publish_cmd_(false)
|
||||
, publish_wheel_joint_controller_state_(false)
|
||||
{
|
||||
}
|
||||
|
||||
bool DiffDriveController::init(hardware_interface::VelocityJointInterface* hw,
|
||||
ros::NodeHandle& root_nh,
|
||||
ros::NodeHandle &controller_nh)
|
||||
{
|
||||
const std::string complete_ns = controller_nh.getNamespace();
|
||||
std::size_t id = complete_ns.find_last_of("/");
|
||||
name_ = complete_ns.substr(id + 1);
|
||||
|
||||
// Get joint names from the parameter server
|
||||
std::vector<std::string> left_wheel_names, right_wheel_names;
|
||||
if (!getWheelNames(controller_nh, "left_wheel", left_wheel_names) ||
|
||||
!getWheelNames(controller_nh, "right_wheel", right_wheel_names))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (left_wheel_names.size() != right_wheel_names.size())
|
||||
{
|
||||
ROS_ERROR_STREAM_NAMED(name_,
|
||||
"#left wheels (" << left_wheel_names.size() << ") != " <<
|
||||
"#right wheels (" << right_wheel_names.size() << ").");
|
||||
return false;
|
||||
}
|
||||
else
|
||||
{
|
||||
wheel_joints_size_ = left_wheel_names.size();
|
||||
|
||||
left_wheel_joints_.resize(wheel_joints_size_);
|
||||
right_wheel_joints_.resize(wheel_joints_size_);
|
||||
}
|
||||
|
||||
// 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("open_loop", open_loop_, open_loop_);
|
||||
|
||||
controller_nh.param("wheel_separation_multiplier", wheel_separation_multiplier_, wheel_separation_multiplier_);
|
||||
ROS_INFO_STREAM_NAMED(name_, "Wheel separation will be multiplied by "
|
||||
<< wheel_separation_multiplier_ << ".");
|
||||
|
||||
if (controller_nh.hasParam("wheel_radius_multiplier"))
|
||||
{
|
||||
double wheel_radius_multiplier;
|
||||
controller_nh.getParam("wheel_radius_multiplier", wheel_radius_multiplier);
|
||||
|
||||
left_wheel_radius_multiplier_ = wheel_radius_multiplier;
|
||||
right_wheel_radius_multiplier_ = wheel_radius_multiplier;
|
||||
}
|
||||
else
|
||||
{
|
||||
controller_nh.param("left_wheel_radius_multiplier", left_wheel_radius_multiplier_, left_wheel_radius_multiplier_);
|
||||
controller_nh.param("right_wheel_radius_multiplier", right_wheel_radius_multiplier_, right_wheel_radius_multiplier_);
|
||||
}
|
||||
|
||||
ROS_INFO_STREAM_NAMED(name_, "Left wheel radius will be multiplied by "
|
||||
<< left_wheel_radius_multiplier_ << ".");
|
||||
ROS_INFO_STREAM_NAMED(name_, "Right wheel radius will be multiplied by "
|
||||
<< right_wheel_radius_multiplier_ << ".");
|
||||
|
||||
int velocity_rolling_window_size = 10;
|
||||
controller_nh.param("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("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("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("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("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("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("linear/x/has_velocity_limits" , limiter_lin_.has_velocity_limits , limiter_lin_.has_velocity_limits );
|
||||
controller_nh.param("linear/x/has_acceleration_limits", limiter_lin_.has_acceleration_limits, limiter_lin_.has_acceleration_limits);
|
||||
controller_nh.param("linear/x/has_jerk_limits" , limiter_lin_.has_jerk_limits , limiter_lin_.has_jerk_limits );
|
||||
controller_nh.param("linear/x/max_velocity" , limiter_lin_.max_velocity , limiter_lin_.max_velocity );
|
||||
controller_nh.param("linear/x/min_velocity" , limiter_lin_.min_velocity , -limiter_lin_.max_velocity );
|
||||
controller_nh.param("linear/x/max_acceleration" , limiter_lin_.max_acceleration , limiter_lin_.max_acceleration );
|
||||
controller_nh.param("linear/x/min_acceleration" , limiter_lin_.min_acceleration , -limiter_lin_.max_acceleration );
|
||||
controller_nh.param("linear/x/max_jerk" , limiter_lin_.max_jerk , limiter_lin_.max_jerk );
|
||||
controller_nh.param("linear/x/min_jerk" , limiter_lin_.min_jerk , -limiter_lin_.max_jerk );
|
||||
|
||||
controller_nh.param("angular/z/has_velocity_limits" , limiter_ang_.has_velocity_limits , limiter_ang_.has_velocity_limits );
|
||||
controller_nh.param("angular/z/has_acceleration_limits", limiter_ang_.has_acceleration_limits, limiter_ang_.has_acceleration_limits);
|
||||
controller_nh.param("angular/z/has_jerk_limits" , limiter_ang_.has_jerk_limits , limiter_ang_.has_jerk_limits );
|
||||
controller_nh.param("angular/z/max_velocity" , limiter_ang_.max_velocity , limiter_ang_.max_velocity );
|
||||
controller_nh.param("angular/z/min_velocity" , limiter_ang_.min_velocity , -limiter_ang_.max_velocity );
|
||||
controller_nh.param("angular/z/max_acceleration" , limiter_ang_.max_acceleration , limiter_ang_.max_acceleration );
|
||||
controller_nh.param("angular/z/min_acceleration" , limiter_ang_.min_acceleration , -limiter_ang_.max_acceleration );
|
||||
controller_nh.param("angular/z/max_jerk" , limiter_ang_.max_jerk , limiter_ang_.max_jerk );
|
||||
controller_nh.param("angular/z/min_jerk" , limiter_ang_.min_jerk , -limiter_ang_.max_jerk );
|
||||
|
||||
// Publish limited velocity:
|
||||
controller_nh.param("publish_cmd", publish_cmd_, publish_cmd_);
|
||||
|
||||
// Publish wheel data:
|
||||
controller_nh.param("publish_wheel_joint_controller_state", publish_wheel_joint_controller_state_, publish_wheel_joint_controller_state_);
|
||||
|
||||
// If either parameter is not available, we need to look up the value in the URDF
|
||||
bool lookup_wheel_separation = !controller_nh.getParam("wheel_separation", wheel_separation_);
|
||||
bool lookup_wheel_radius = !controller_nh.getParam("wheel_radius", wheel_radius_);
|
||||
|
||||
if (!setOdomParamsFromUrdf(root_nh,
|
||||
left_wheel_names[0],
|
||||
right_wheel_names[0],
|
||||
lookup_wheel_separation,
|
||||
lookup_wheel_radius))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// Regardless of how we got the separation and radius, use them
|
||||
// to set the odometry parameters
|
||||
const double ws = wheel_separation_multiplier_ * wheel_separation_;
|
||||
const double lwr = left_wheel_radius_multiplier_ * wheel_radius_;
|
||||
const double rwr = right_wheel_radius_multiplier_ * wheel_radius_;
|
||||
odometry_.setWheelParams(ws, lwr, rwr);
|
||||
ROS_INFO_STREAM_NAMED(name_,
|
||||
"Odometry params : wheel separation " << ws
|
||||
<< ", left wheel radius " << lwr
|
||||
<< ", right wheel radius " << rwr);
|
||||
|
||||
if (publish_cmd_)
|
||||
{
|
||||
cmd_vel_pub_.reset(new realtime_tools::RealtimePublisher<geometry_msgs::TwistStamped>(controller_nh, "cmd_vel_out", 100));
|
||||
}
|
||||
|
||||
// Wheel joint controller state:
|
||||
if (publish_wheel_joint_controller_state_)
|
||||
{
|
||||
controller_state_pub_.reset(new realtime_tools::RealtimePublisher<control_msgs::JointTrajectoryControllerState>(controller_nh, "wheel_joint_controller_state", 100));
|
||||
|
||||
const size_t num_wheels = wheel_joints_size_ * 2;
|
||||
|
||||
controller_state_pub_->msg_.joint_names.resize(num_wheels);
|
||||
|
||||
controller_state_pub_->msg_.desired.positions.resize(num_wheels);
|
||||
controller_state_pub_->msg_.desired.velocities.resize(num_wheels);
|
||||
controller_state_pub_->msg_.desired.accelerations.resize(num_wheels);
|
||||
controller_state_pub_->msg_.desired.effort.resize(num_wheels);
|
||||
|
||||
controller_state_pub_->msg_.actual.positions.resize(num_wheels);
|
||||
controller_state_pub_->msg_.actual.velocities.resize(num_wheels);
|
||||
controller_state_pub_->msg_.actual.accelerations.resize(num_wheels);
|
||||
controller_state_pub_->msg_.actual.effort.resize(num_wheels);
|
||||
|
||||
controller_state_pub_->msg_.error.positions.resize(num_wheels);
|
||||
controller_state_pub_->msg_.error.velocities.resize(num_wheels);
|
||||
controller_state_pub_->msg_.error.accelerations.resize(num_wheels);
|
||||
controller_state_pub_->msg_.error.effort.resize(num_wheels);
|
||||
|
||||
for (size_t i = 0; i < wheel_joints_size_; ++i)
|
||||
{
|
||||
controller_state_pub_->msg_.joint_names[i] = left_wheel_names[i];
|
||||
controller_state_pub_->msg_.joint_names[i + wheel_joints_size_] = right_wheel_names[i];
|
||||
}
|
||||
|
||||
vel_left_previous_.resize(wheel_joints_size_, 0.0);
|
||||
vel_right_previous_.resize(wheel_joints_size_, 0.0);
|
||||
}
|
||||
|
||||
setOdomPubFields(root_nh, controller_nh);
|
||||
|
||||
// Get the joint object to use in the realtime loop
|
||||
for (size_t i = 0; i < wheel_joints_size_; ++i)
|
||||
{
|
||||
ROS_INFO_STREAM_NAMED(name_,
|
||||
"Adding left wheel with joint name: " << left_wheel_names[i]
|
||||
<< " and right wheel with joint name: " << right_wheel_names[i]);
|
||||
left_wheel_joints_[i] = hw->getHandle(left_wheel_names[i]); // throws on failure
|
||||
right_wheel_joints_[i] = hw->getHandle(right_wheel_names[i]); // throws on failure
|
||||
}
|
||||
|
||||
sub_command_ = controller_nh.subscribe("cmd_vel", 1, &DiffDriveController::cmdVelCallback, this);
|
||||
|
||||
// Initialize dynamic parameters
|
||||
DynamicParams dynamic_params;
|
||||
dynamic_params.left_wheel_radius_multiplier = left_wheel_radius_multiplier_;
|
||||
dynamic_params.right_wheel_radius_multiplier = right_wheel_radius_multiplier_;
|
||||
dynamic_params.wheel_separation_multiplier = wheel_separation_multiplier_;
|
||||
|
||||
dynamic_params.publish_rate = publish_rate;
|
||||
dynamic_params.enable_odom_tf = enable_odom_tf_;
|
||||
|
||||
dynamic_params_.writeFromNonRT(dynamic_params);
|
||||
|
||||
// Initialize dynamic_reconfigure server
|
||||
DiffDriveControllerConfig config;
|
||||
config.left_wheel_radius_multiplier = left_wheel_radius_multiplier_;
|
||||
config.right_wheel_radius_multiplier = right_wheel_radius_multiplier_;
|
||||
config.wheel_separation_multiplier = wheel_separation_multiplier_;
|
||||
|
||||
config.publish_rate = publish_rate;
|
||||
config.enable_odom_tf = enable_odom_tf_;
|
||||
|
||||
dyn_reconf_server_ = std::make_shared<ReconfigureServer>(dyn_reconf_server_mutex_, controller_nh);
|
||||
|
||||
// Update parameters
|
||||
dyn_reconf_server_mutex_.lock();
|
||||
dyn_reconf_server_->updateConfig(config);
|
||||
dyn_reconf_server_mutex_.unlock();
|
||||
|
||||
dyn_reconf_server_->setCallback(
|
||||
std::bind(&DiffDriveController::reconfCallback, this, std::placeholders::_1, std::placeholders::_2));
|
||||
return true;
|
||||
}
|
||||
|
||||
void DiffDriveController::update(const ros::Time& time, const ros::Duration& period)
|
||||
{
|
||||
// update parameter from dynamic reconf
|
||||
updateDynamicParams();
|
||||
|
||||
// Apply (possibly new) multipliers:
|
||||
const double ws = wheel_separation_multiplier_ * wheel_separation_;
|
||||
const double lwr = left_wheel_radius_multiplier_ * wheel_radius_;
|
||||
const double rwr = right_wheel_radius_multiplier_ * wheel_radius_;
|
||||
|
||||
odometry_.setWheelParams(ws, lwr, rwr);
|
||||
|
||||
// COMPUTE AND PUBLISH ODOMETRY
|
||||
if (open_loop_)
|
||||
{
|
||||
odometry_.updateOpenLoop(last0_cmd_.lin, last0_cmd_.ang, time);
|
||||
}
|
||||
else
|
||||
{
|
||||
double left_pos = 0.0;
|
||||
double right_pos = 0.0;
|
||||
for (size_t i = 0; i < wheel_joints_size_; ++i)
|
||||
{
|
||||
const double lp = left_wheel_joints_[i].getPosition();
|
||||
const double rp = right_wheel_joints_[i].getPosition();
|
||||
if (std::isnan(lp) || std::isnan(rp))
|
||||
return;
|
||||
|
||||
left_pos += lp;
|
||||
right_pos += rp;
|
||||
}
|
||||
left_pos /= wheel_joints_size_;
|
||||
right_pos /= wheel_joints_size_;
|
||||
|
||||
// Estimate linear and angular velocity using joint information
|
||||
odometry_.update(left_pos, right_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());
|
||||
|
||||
limiter_lin_.limit(curr_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, cmd_dt);
|
||||
limiter_ang_.limit(curr_cmd.ang, last0_cmd_.ang, last1_cmd_.ang, cmd_dt);
|
||||
|
||||
last1_cmd_ = last0_cmd_;
|
||||
last0_cmd_ = curr_cmd;
|
||||
|
||||
// Publish limited velocity:
|
||||
if (publish_cmd_ && cmd_vel_pub_ && cmd_vel_pub_->trylock())
|
||||
{
|
||||
cmd_vel_pub_->msg_.header.stamp = time;
|
||||
cmd_vel_pub_->msg_.twist.linear.x = curr_cmd.lin;
|
||||
cmd_vel_pub_->msg_.twist.angular.z = curr_cmd.ang;
|
||||
cmd_vel_pub_->unlockAndPublish();
|
||||
}
|
||||
|
||||
// Compute wheels velocities:
|
||||
const double vel_left = (curr_cmd.lin - curr_cmd.ang * ws / 2.0)/lwr;
|
||||
const double vel_right = (curr_cmd.lin + curr_cmd.ang * ws / 2.0)/rwr;
|
||||
|
||||
// Set wheels velocities:
|
||||
for (size_t i = 0; i < wheel_joints_size_; ++i)
|
||||
{
|
||||
left_wheel_joints_[i].setCommand(vel_left);
|
||||
right_wheel_joints_[i].setCommand(vel_right);
|
||||
}
|
||||
|
||||
publishWheelData(time, period, curr_cmd, ws, lwr, rwr);
|
||||
time_previous_ = time;
|
||||
}
|
||||
|
||||
void DiffDriveController::starting(const ros::Time& time)
|
||||
{
|
||||
brake();
|
||||
|
||||
// Register starting time used to keep fixed rate
|
||||
last_state_publish_time_ = time;
|
||||
time_previous_ = time;
|
||||
|
||||
odometry_.init(time);
|
||||
}
|
||||
|
||||
void DiffDriveController::stopping(const ros::Time& /*time*/)
|
||||
{
|
||||
brake();
|
||||
}
|
||||
|
||||
void DiffDriveController::brake()
|
||||
{
|
||||
const double vel = 0.0;
|
||||
for (size_t i = 0; i < wheel_joints_size_; ++i)
|
||||
{
|
||||
left_wheel_joints_[i].setCommand(vel);
|
||||
right_wheel_joints_[i].setCommand(vel);
|
||||
}
|
||||
}
|
||||
|
||||
void DiffDriveController::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;
|
||||
}
|
||||
|
||||
if(!std::isfinite(command.angular.z) || !std::isfinite(command.linear.x))
|
||||
{
|
||||
ROS_WARN_THROTTLE(1.0, "Received NaN in velocity command. Ignoring.");
|
||||
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 DiffDriveController::getWheelNames(ros::NodeHandle& controller_nh,
|
||||
const std::string& wheel_param,
|
||||
std::vector<std::string>& wheel_names)
|
||||
{
|
||||
XmlRpc::XmlRpcValue wheel_list;
|
||||
if (!controller_nh.getParam(wheel_param, wheel_list))
|
||||
{
|
||||
ROS_ERROR_STREAM_NAMED(name_,
|
||||
"Couldn't retrieve wheel param '" << wheel_param << "'.");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeArray)
|
||||
{
|
||||
if (wheel_list.size() == 0)
|
||||
{
|
||||
ROS_ERROR_STREAM_NAMED(name_,
|
||||
"Wheel param '" << wheel_param << "' is an empty list");
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int i = 0; i < wheel_list.size(); ++i)
|
||||
{
|
||||
if (wheel_list[i].getType() != XmlRpc::XmlRpcValue::TypeString)
|
||||
{
|
||||
ROS_ERROR_STREAM_NAMED(name_,
|
||||
"Wheel param '" << wheel_param << "' #" << i <<
|
||||
" isn't a string.");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
wheel_names.resize(wheel_list.size());
|
||||
for (int i = 0; i < wheel_list.size(); ++i)
|
||||
{
|
||||
wheel_names[i] = static_cast<std::string>(wheel_list[i]);
|
||||
}
|
||||
}
|
||||
else if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeString)
|
||||
{
|
||||
wheel_names.push_back(wheel_list);
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR_STREAM_NAMED(name_,
|
||||
"Wheel param '" << wheel_param <<
|
||||
"' is neither a list of strings nor a string.");
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool DiffDriveController::setOdomParamsFromUrdf(ros::NodeHandle& root_nh,
|
||||
const std::string& left_wheel_name,
|
||||
const std::string& right_wheel_name,
|
||||
bool lookup_wheel_separation,
|
||||
bool lookup_wheel_radius)
|
||||
{
|
||||
if (!(lookup_wheel_separation || 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 description couldn't be retrieved from param server.");
|
||||
return false;
|
||||
}
|
||||
|
||||
urdf::ModelInterfaceSharedPtr model(urdf::parseURDF(robot_model_str));
|
||||
|
||||
urdf::JointConstSharedPtr left_wheel_joint(model->getJoint(left_wheel_name));
|
||||
urdf::JointConstSharedPtr right_wheel_joint(model->getJoint(right_wheel_name));
|
||||
|
||||
if (!left_wheel_joint)
|
||||
{
|
||||
ROS_ERROR_STREAM_NAMED(name_, left_wheel_name
|
||||
<< " couldn't be retrieved from model description");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!right_wheel_joint)
|
||||
{
|
||||
ROS_ERROR_STREAM_NAMED(name_, right_wheel_name
|
||||
<< " couldn't be retrieved from model description");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (lookup_wheel_separation)
|
||||
{
|
||||
// Get wheel separation
|
||||
ROS_INFO_STREAM("left wheel to origin: " << left_wheel_joint->parent_to_joint_origin_transform.position.x << ","
|
||||
<< left_wheel_joint->parent_to_joint_origin_transform.position.y << ", "
|
||||
<< left_wheel_joint->parent_to_joint_origin_transform.position.z);
|
||||
ROS_INFO_STREAM("right wheel to origin: " << right_wheel_joint->parent_to_joint_origin_transform.position.x << ","
|
||||
<< right_wheel_joint->parent_to_joint_origin_transform.position.y << ", "
|
||||
<< right_wheel_joint->parent_to_joint_origin_transform.position.z);
|
||||
|
||||
wheel_separation_ = euclideanOfVectors(left_wheel_joint->parent_to_joint_origin_transform.position,
|
||||
right_wheel_joint->parent_to_joint_origin_transform.position);
|
||||
|
||||
ROS_INFO_STREAM("wheel_separation : " << wheel_separation_);
|
||||
|
||||
}
|
||||
|
||||
if (lookup_wheel_radius)
|
||||
{
|
||||
// Get wheel radius
|
||||
if (!getWheelRadius(model->getLink(left_wheel_joint->child_link_name), wheel_radius_))
|
||||
{
|
||||
ROS_ERROR_STREAM_NAMED(name_, "Couldn't retrieve " << left_wheel_name << " wheel radius");
|
||||
return false;
|
||||
}
|
||||
else ROS_INFO("wheel radius : %f", wheel_radius_);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void DiffDriveController::setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh)
|
||||
{
|
||||
// Get and check params for covariances
|
||||
XmlRpc::XmlRpcValue pose_cov_list;
|
||||
controller_nh.getParam("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("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, "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 = {
|
||||
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 = {
|
||||
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_;
|
||||
}
|
||||
|
||||
void DiffDriveController::reconfCallback(DiffDriveControllerConfig& config, uint32_t /*level*/)
|
||||
{
|
||||
DynamicParams dynamic_params;
|
||||
dynamic_params.left_wheel_radius_multiplier = config.left_wheel_radius_multiplier;
|
||||
dynamic_params.right_wheel_radius_multiplier = config.right_wheel_radius_multiplier;
|
||||
dynamic_params.wheel_separation_multiplier = config.wheel_separation_multiplier;
|
||||
|
||||
dynamic_params.publish_rate = config.publish_rate;
|
||||
|
||||
dynamic_params.enable_odom_tf = config.enable_odom_tf;
|
||||
|
||||
dynamic_params_.writeFromNonRT(dynamic_params);
|
||||
|
||||
ROS_INFO_STREAM_NAMED(name_, "Dynamic Reconfigure:\n" << dynamic_params);
|
||||
}
|
||||
|
||||
void DiffDriveController::updateDynamicParams()
|
||||
{
|
||||
// Retreive dynamic params:
|
||||
const DynamicParams dynamic_params = *(dynamic_params_.readFromRT());
|
||||
|
||||
left_wheel_radius_multiplier_ = dynamic_params.left_wheel_radius_multiplier;
|
||||
right_wheel_radius_multiplier_ = dynamic_params.right_wheel_radius_multiplier;
|
||||
wheel_separation_multiplier_ = dynamic_params.wheel_separation_multiplier;
|
||||
|
||||
publish_period_ = ros::Duration(1.0 / dynamic_params.publish_rate);
|
||||
enable_odom_tf_ = dynamic_params.enable_odom_tf;
|
||||
}
|
||||
|
||||
void DiffDriveController::publishWheelData(const ros::Time& time, const ros::Duration& period, Commands& curr_cmd,
|
||||
double wheel_separation, double left_wheel_radius, double right_wheel_radius)
|
||||
{
|
||||
if (publish_wheel_joint_controller_state_ && controller_state_pub_->trylock())
|
||||
{
|
||||
const double cmd_dt(period.toSec());
|
||||
|
||||
// Compute desired wheels velocities, that is before applying limits:
|
||||
const double vel_left_desired = (curr_cmd.lin - curr_cmd.ang * wheel_separation / 2.0) / left_wheel_radius;
|
||||
const double vel_right_desired = (curr_cmd.lin + curr_cmd.ang * wheel_separation / 2.0) / right_wheel_radius;
|
||||
controller_state_pub_->msg_.header.stamp = time;
|
||||
|
||||
for (size_t i = 0; i < wheel_joints_size_; ++i)
|
||||
{
|
||||
const double control_duration = (time - time_previous_).toSec();
|
||||
|
||||
const double left_wheel_acc = (left_wheel_joints_[i].getVelocity() - vel_left_previous_[i]) / control_duration;
|
||||
const double right_wheel_acc = (right_wheel_joints_[i].getVelocity() - vel_right_previous_[i]) / control_duration;
|
||||
|
||||
// Actual
|
||||
controller_state_pub_->msg_.actual.positions[i] = left_wheel_joints_[i].getPosition();
|
||||
controller_state_pub_->msg_.actual.velocities[i] = left_wheel_joints_[i].getVelocity();
|
||||
controller_state_pub_->msg_.actual.accelerations[i] = left_wheel_acc;
|
||||
controller_state_pub_->msg_.actual.effort[i] = left_wheel_joints_[i].getEffort();
|
||||
|
||||
controller_state_pub_->msg_.actual.positions[i + wheel_joints_size_] = right_wheel_joints_[i].getPosition();
|
||||
controller_state_pub_->msg_.actual.velocities[i + wheel_joints_size_] = right_wheel_joints_[i].getVelocity();
|
||||
controller_state_pub_->msg_.actual.accelerations[i + wheel_joints_size_] = right_wheel_acc;
|
||||
controller_state_pub_->msg_.actual.effort[i+ wheel_joints_size_] = right_wheel_joints_[i].getEffort();
|
||||
|
||||
// Desired
|
||||
controller_state_pub_->msg_.desired.positions[i] += vel_left_desired * cmd_dt;
|
||||
controller_state_pub_->msg_.desired.velocities[i] = vel_left_desired;
|
||||
controller_state_pub_->msg_.desired.accelerations[i] = (vel_left_desired - vel_left_desired_previous_) * cmd_dt;
|
||||
controller_state_pub_->msg_.desired.effort[i] = std::numeric_limits<double>::quiet_NaN();
|
||||
|
||||
controller_state_pub_->msg_.desired.positions[i + wheel_joints_size_] += vel_right_desired * cmd_dt;
|
||||
controller_state_pub_->msg_.desired.velocities[i + wheel_joints_size_] = vel_right_desired;
|
||||
controller_state_pub_->msg_.desired.accelerations[i + wheel_joints_size_] = (vel_right_desired - vel_right_desired_previous_) * cmd_dt;
|
||||
controller_state_pub_->msg_.desired.effort[i+ wheel_joints_size_] = std::numeric_limits<double>::quiet_NaN();
|
||||
|
||||
// Error
|
||||
controller_state_pub_->msg_.error.positions[i] = controller_state_pub_->msg_.desired.positions[i] -
|
||||
controller_state_pub_->msg_.actual.positions[i];
|
||||
controller_state_pub_->msg_.error.velocities[i] = controller_state_pub_->msg_.desired.velocities[i] -
|
||||
controller_state_pub_->msg_.actual.velocities[i];
|
||||
controller_state_pub_->msg_.error.accelerations[i] = controller_state_pub_->msg_.desired.accelerations[i] -
|
||||
controller_state_pub_->msg_.actual.accelerations[i];
|
||||
controller_state_pub_->msg_.error.effort[i] = controller_state_pub_->msg_.desired.effort[i] -
|
||||
controller_state_pub_->msg_.actual.effort[i];
|
||||
|
||||
controller_state_pub_->msg_.error.positions[i + wheel_joints_size_] = controller_state_pub_->msg_.desired.positions[i + wheel_joints_size_] -
|
||||
controller_state_pub_->msg_.actual.positions[i + wheel_joints_size_];
|
||||
controller_state_pub_->msg_.error.velocities[i + wheel_joints_size_] = controller_state_pub_->msg_.desired.velocities[i + wheel_joints_size_] -
|
||||
controller_state_pub_->msg_.actual.velocities[i + wheel_joints_size_];
|
||||
controller_state_pub_->msg_.error.accelerations[i + wheel_joints_size_] = controller_state_pub_->msg_.desired.accelerations[i + wheel_joints_size_] -
|
||||
controller_state_pub_->msg_.actual.accelerations[i + wheel_joints_size_];
|
||||
controller_state_pub_->msg_.error.effort[i+ wheel_joints_size_] = controller_state_pub_->msg_.desired.effort[i + wheel_joints_size_] -
|
||||
controller_state_pub_->msg_.actual.effort[i + wheel_joints_size_];
|
||||
|
||||
// Save previous velocities to compute acceleration
|
||||
vel_left_previous_[i] = left_wheel_joints_[i].getVelocity();
|
||||
vel_right_previous_[i] = right_wheel_joints_[i].getVelocity();
|
||||
vel_left_desired_previous_ = vel_left_desired;
|
||||
vel_right_desired_previous_ = vel_right_desired;
|
||||
}
|
||||
|
||||
controller_state_pub_->unlockAndPublish();
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace diff_drive_controller
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(diff_drive_controller::DiffDriveController, controller_interface::ControllerBase);
|
||||
174
Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/src/odometry.cpp
Executable file
174
Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/src/odometry.cpp
Executable file
@@ -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 <diff_drive_controller/odometry.h>
|
||||
|
||||
namespace diff_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_(0.0)
|
||||
, left_wheel_radius_(0.0)
|
||||
, right_wheel_radius_(0.0)
|
||||
, left_wheel_old_pos_(0.0)
|
||||
, right_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_(std::bind(&Odometry::integrateExact, this, std::placeholders::_1, std::placeholders::_2))
|
||||
{
|
||||
}
|
||||
|
||||
void Odometry::init(const ros::Time& time)
|
||||
{
|
||||
// Reset accumulators and timestamp:
|
||||
resetAccumulators();
|
||||
timestamp_ = time;
|
||||
}
|
||||
|
||||
bool Odometry::update(double left_pos, double right_pos, const ros::Time &time)
|
||||
{
|
||||
/// Get current wheel joint positions:
|
||||
const double left_wheel_cur_pos = left_pos * left_wheel_radius_;
|
||||
const double right_wheel_cur_pos = right_pos * right_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_;
|
||||
|
||||
/// Update old position with current:
|
||||
left_wheel_old_pos_ = left_wheel_cur_pos;
|
||||
right_wheel_old_pos_ = right_wheel_cur_pos;
|
||||
|
||||
/// Compute linear and angular diff:
|
||||
const double linear = (right_wheel_est_vel + left_wheel_est_vel) * 0.5 ;
|
||||
const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_;
|
||||
|
||||
/// 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, double left_wheel_radius, double right_wheel_radius)
|
||||
{
|
||||
wheel_separation_ = wheel_separation;
|
||||
left_wheel_radius_ = left_wheel_radius;
|
||||
right_wheel_radius_ = right_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 <diff_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 diff_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,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
|
||||
|
||||
<!-- Override robot_description with bad urdf -->
|
||||
<param name="robot_description"
|
||||
command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot_bad.xacro'" />
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="diff_drive_fail_test"
|
||||
pkg="diff_drive_controller"
|
||||
type="diff_drive_fail_test"
|
||||
time-limit="10.0">
|
||||
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="diffbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,35 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Use simulation time -->
|
||||
<rosparam param="use_sim_time">True</rosparam>
|
||||
|
||||
<!-- Load diffbot model -->
|
||||
<param name="robot_description"
|
||||
command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot.xacro'" />
|
||||
|
||||
<!-- Start diffbot -->
|
||||
<node name="diffbot"
|
||||
pkg="diff_drive_controller"
|
||||
type="diffbot"/>
|
||||
|
||||
<!-- Load controller config -->
|
||||
<rosparam command="load" file="$(find diff_drive_controller)/test/diffbot_controllers.yaml" />
|
||||
|
||||
<!-- Spawn controller -->
|
||||
<node name="controller_spawner"
|
||||
pkg="controller_manager" type="spawner" output="screen"
|
||||
args="diffbot_controller" />
|
||||
|
||||
<!-- rqt_plot monitoring -->
|
||||
<!--
|
||||
<node name="diffbot_pos_monitor"
|
||||
pkg="rqt_plot"
|
||||
type="rqt_plot"
|
||||
args="/diffbot_controller/odom/pose/pose/position/x" />
|
||||
|
||||
<node name="diffbot_vel_monitor"
|
||||
pkg="rqt_plot"
|
||||
type="rqt_plot"
|
||||
args="/diffbot_controller/odom/twist/twist/linear/x" />
|
||||
-->
|
||||
</launch>
|
||||
@@ -0,0 +1,14 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="diff_drive_test"
|
||||
pkg="diff_drive_controller"
|
||||
type="diff_drive_test"
|
||||
time-limit="80.0">
|
||||
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="diffbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,17 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
|
||||
|
||||
<!-- Load diff-drive limits -->
|
||||
<rosparam command="load" file="$(find diff_drive_controller)/test/diffbot_limits.yaml" />
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="diff_drive_limits_test"
|
||||
pkg="diff_drive_controller"
|
||||
type="diff_drive_limits_test"
|
||||
time-limit="80.0">
|
||||
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="diffbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,20 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
|
||||
|
||||
<rosparam>
|
||||
diffbot_controller:
|
||||
publish_cmd: True
|
||||
</rosparam>
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="diff_drive_nan_test"
|
||||
pkg="diff_drive_controller"
|
||||
type="diff_drive_nan_test"
|
||||
time-limit="80.0">
|
||||
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="diffbot_controller/odom" />
|
||||
<remap from="cmd_vel_out" to="diffbot_controller/cmd_vel_out" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,21 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
|
||||
|
||||
<!-- default value -->
|
||||
<!-- <rosparam>
|
||||
diffbot_controller:
|
||||
publish_cmd: False
|
||||
</rosparam> -->
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="diff_drive_default_cmd_vel_out_test"
|
||||
pkg="diff_drive_controller"
|
||||
type="diff_drive_default_cmd_vel_out_test"
|
||||
time-limit="80.0">
|
||||
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="diffbot_controller/odom" />
|
||||
<remap from="cmd_vel_out" to="diffbot_controller/cmd_vel_out" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,69 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// Copyright (C) 2017, PAL Robotics.
|
||||
//
|
||||
// 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 Jeremie Deray
|
||||
|
||||
#include "test_common.h"
|
||||
|
||||
// TEST CASES
|
||||
TEST_F(DiffDriveControllerTest, testDefaultCmdVelOutTopic)
|
||||
{
|
||||
// wait for ROS
|
||||
waitForController();
|
||||
// msgs are published in the same loop
|
||||
// thus if odom is published cmd_vel_out
|
||||
// should be as well (if enabled)
|
||||
waitForOdomMsgs();
|
||||
|
||||
EXPECT_FALSE(isPublishingCmdVelOut());
|
||||
|
||||
// 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();
|
||||
|
||||
cmd_vel.linear.x = 0.1;
|
||||
publish(cmd_vel);
|
||||
ros::Duration(0.1).sleep();
|
||||
|
||||
EXPECT_FALSE(isPublishingCmdVelOut());
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
ros::init(argc, argv, "diff_drive_default_cmd_vel_out_topic_test");
|
||||
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
int ret = RUN_ALL_TESTS();
|
||||
spinner.stop();
|
||||
ros::shutdown();
|
||||
return ret;
|
||||
}
|
||||
@@ -0,0 +1,20 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
|
||||
|
||||
<!-- Do not specific an odom_frame_id -->
|
||||
<!-- <rosparam>
|
||||
diffbot_controller:
|
||||
odom_frame_id: odom
|
||||
</rosparam> -->
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="diff_drive_default_odom_frame_test"
|
||||
pkg="diff_drive_controller"
|
||||
type="diff_drive_default_odom_frame_test"
|
||||
time-limit="80.0">
|
||||
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="diffbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,69 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// 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 "test_common.h"
|
||||
#include <tf/transform_listener.h>
|
||||
|
||||
// TEST CASES
|
||||
TEST_F(DiffDriveControllerTest, testOdomFrame)
|
||||
{
|
||||
// wait for ROS
|
||||
waitForController();
|
||||
|
||||
// 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(DiffDriveControllerTest, testOdomTopic)
|
||||
{
|
||||
// wait for ROS
|
||||
waitForController();
|
||||
waitForOdomMsgs();
|
||||
|
||||
// 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, "diff_drive_default_odom_frame_test");
|
||||
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
int ret = RUN_ALL_TESTS();
|
||||
spinner.stop();
|
||||
ros::shutdown();
|
||||
return ret;
|
||||
}
|
||||
@@ -0,0 +1,21 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
|
||||
|
||||
<!-- default value -->
|
||||
<!-- <rosparam>
|
||||
diffbot_controller:
|
||||
publish_wheel_joint_controller_state: False
|
||||
</rosparam> -->
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="diff_drive_default_wheel_joint_controller_state_test"
|
||||
pkg="diff_drive_controller"
|
||||
type="diff_drive_default_wheel_joint_controller_state_test"
|
||||
time-limit="30.0">
|
||||
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="diffbot_controller/odom" />
|
||||
<remap from="wheel_joint_controller_state" to="diffbot_controller/wheel_joint_controller_state" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,65 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// Copyright (C) 2018, PAL Robotics.
|
||||
//
|
||||
// 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 Jeremie Deray
|
||||
|
||||
#include "test_common.h"
|
||||
|
||||
// TEST CASES
|
||||
TEST_F(DiffDriveControllerTest, testDefaultJointTrajectoryControllerStateTopic)
|
||||
{
|
||||
// wait for ROS
|
||||
waitForController();
|
||||
|
||||
EXPECT_FALSE(isPublishingJointTrajectoryControllerState());
|
||||
|
||||
// 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();
|
||||
|
||||
cmd_vel.linear.x = 0.1;
|
||||
publish(cmd_vel);
|
||||
ros::Duration(0.1).sleep();
|
||||
|
||||
EXPECT_FALSE(isPublishingJointTrajectoryControllerState());
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
ros::init(argc, argv, "diff_drive_default_wheel_joint_controller_state_topic_test");
|
||||
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
int ret = RUN_ALL_TESTS();
|
||||
spinner.stop();
|
||||
ros::shutdown();
|
||||
return ret;
|
||||
}
|
||||
@@ -0,0 +1,20 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
|
||||
|
||||
<!-- Make sure to disable odom tf -->
|
||||
<rosparam>
|
||||
diffbot_controller:
|
||||
enable_odom_tf: False
|
||||
</rosparam>
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="diff_drive_dyn_reconf_test"
|
||||
pkg="diff_drive_controller"
|
||||
type="diff_drive_dyn_reconf_test"
|
||||
time-limit="80.0">
|
||||
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="diffbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,176 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// Copyright (C) 2018, 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 Jeremie Deray
|
||||
|
||||
#include "test_common.h"
|
||||
#include <tf/transform_listener.h>
|
||||
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
|
||||
// TEST CASES
|
||||
TEST_F(DiffDriveControllerTest, testDynReconfServerAlive)
|
||||
{
|
||||
// wait for ROS
|
||||
waitForController();
|
||||
|
||||
// Expect server is alive
|
||||
EXPECT_TRUE(ros::service::exists("diffbot_controller/set_parameters", true));
|
||||
|
||||
dynamic_reconfigure::ReconfigureRequest srv_req;
|
||||
dynamic_reconfigure::ReconfigureResponse srv_resp;
|
||||
|
||||
// Expect server is callable (get-fashion)
|
||||
EXPECT_TRUE(ros::service::call("diffbot_controller/set_parameters", srv_req, srv_resp));
|
||||
|
||||
EXPECT_EQ(1, srv_resp.config.bools.size());
|
||||
|
||||
if (!srv_resp.config.bools.empty())
|
||||
{
|
||||
EXPECT_EQ("enable_odom_tf", srv_resp.config.bools[0].name);
|
||||
// expect false since it is set to false in the .test
|
||||
EXPECT_EQ(false, srv_resp.config.bools[0].value);
|
||||
}
|
||||
|
||||
EXPECT_EQ(4, srv_resp.config.doubles.size());
|
||||
|
||||
if (srv_resp.config.doubles.size() >= 4)
|
||||
{
|
||||
EXPECT_EQ("left_wheel_radius_multiplier", srv_resp.config.doubles[0].name);
|
||||
EXPECT_EQ(1, srv_resp.config.doubles[0].value);
|
||||
|
||||
EXPECT_EQ("right_wheel_radius_multiplier", srv_resp.config.doubles[1].name);
|
||||
EXPECT_EQ(1, srv_resp.config.doubles[1].value);
|
||||
|
||||
EXPECT_EQ("wheel_separation_multiplier", srv_resp.config.doubles[2].name);
|
||||
EXPECT_EQ(1, srv_resp.config.doubles[2].value);
|
||||
|
||||
EXPECT_EQ("publish_rate", srv_resp.config.doubles[3].name);
|
||||
EXPECT_EQ(50, srv_resp.config.doubles[3].value);
|
||||
}
|
||||
|
||||
dynamic_reconfigure::DoubleParameter double_param;
|
||||
double_param.name = "left_wheel_radius_multiplier";
|
||||
double_param.value = 0.95;
|
||||
|
||||
srv_req.config.doubles.push_back(double_param);
|
||||
|
||||
double_param.name = "right_wheel_radius_multiplier";
|
||||
double_param.value = 0.95;
|
||||
|
||||
srv_req.config.doubles.push_back(double_param);
|
||||
|
||||
double_param.name = "wheel_separation_multiplier";
|
||||
double_param.value = 0.95;
|
||||
|
||||
srv_req.config.doubles.push_back(double_param);
|
||||
|
||||
double_param.name = "publish_rate";
|
||||
double_param.value = 150;
|
||||
|
||||
srv_req.config.doubles.push_back(double_param);
|
||||
|
||||
dynamic_reconfigure::BoolParameter bool_param;
|
||||
bool_param.name = "enable_odom_tf";
|
||||
bool_param.value = false;
|
||||
|
||||
srv_req.config.bools.push_back(bool_param);
|
||||
|
||||
// Expect server is callable (set-fashion)
|
||||
EXPECT_TRUE(ros::service::call("diffbot_controller/set_parameters", srv_req, srv_resp));
|
||||
|
||||
EXPECT_EQ(1, srv_resp.config.bools.size());
|
||||
|
||||
if (!srv_resp.config.bools.empty())
|
||||
{
|
||||
EXPECT_EQ("enable_odom_tf", srv_resp.config.bools[0].name);
|
||||
EXPECT_EQ(false, srv_resp.config.bools[0].value);
|
||||
}
|
||||
|
||||
EXPECT_EQ(4, srv_resp.config.doubles.size());
|
||||
|
||||
if (srv_resp.config.doubles.size() >= 4)
|
||||
{
|
||||
EXPECT_EQ("left_wheel_radius_multiplier", srv_resp.config.doubles[0].name);
|
||||
EXPECT_EQ(0.95, srv_resp.config.doubles[0].value);
|
||||
|
||||
EXPECT_EQ("right_wheel_radius_multiplier", srv_resp.config.doubles[1].name);
|
||||
EXPECT_EQ(0.95, srv_resp.config.doubles[1].value);
|
||||
|
||||
EXPECT_EQ("wheel_separation_multiplier", srv_resp.config.doubles[2].name);
|
||||
EXPECT_EQ(0.95, srv_resp.config.doubles[2].value);
|
||||
|
||||
EXPECT_EQ("publish_rate", srv_resp.config.doubles[3].name);
|
||||
EXPECT_EQ(150, srv_resp.config.doubles[3].value);
|
||||
}
|
||||
}
|
||||
|
||||
// TEST CASES
|
||||
TEST_F(DiffDriveControllerTest, testDynReconfEnableTf)
|
||||
{
|
||||
// wait for ROS
|
||||
while(!isControllerAlive() && ros::ok())
|
||||
{
|
||||
ros::Duration(0.1).sleep();
|
||||
}
|
||||
if (!ros::ok())
|
||||
FAIL() << "Something went wrong while executing test";
|
||||
|
||||
// set up tf listener
|
||||
tf::TransformListener listener;
|
||||
ros::Duration(2.0).sleep();
|
||||
// check the odom frame doesn't exist
|
||||
EXPECT_FALSE(listener.frameExists("odom"));
|
||||
|
||||
dynamic_reconfigure::ReconfigureRequest srv_req;
|
||||
dynamic_reconfigure::ReconfigureResponse srv_resp;
|
||||
|
||||
dynamic_reconfigure::BoolParameter bool_param;
|
||||
bool_param.name = "enable_odom_tf";
|
||||
bool_param.value = true;
|
||||
|
||||
srv_req.config.bools.push_back(bool_param);
|
||||
|
||||
EXPECT_TRUE(ros::service::call("diffbot_controller/set_parameters", srv_req, srv_resp));
|
||||
|
||||
ros::Duration(2.0).sleep();
|
||||
// check the odom frame doesn't exist
|
||||
EXPECT_TRUE(listener.frameExists("odom"));
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
ros::init(argc, argv, "diff_drive_dyn_reconf_test");
|
||||
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
int ret = RUN_ALL_TESTS();
|
||||
spinner.stop();
|
||||
ros::shutdown();
|
||||
return ret;
|
||||
}
|
||||
@@ -0,0 +1,60 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// 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 "test_common.h"
|
||||
|
||||
// TEST CASES
|
||||
TEST_F(DiffDriveControllerTest, testWrongJointName)
|
||||
{
|
||||
// the controller should never be alive
|
||||
int secs = 0;
|
||||
while(!isControllerAlive() && ros::ok() && secs < 5)
|
||||
{
|
||||
ros::Duration(1.0).sleep();
|
||||
secs++;
|
||||
}
|
||||
if (!ros::ok())
|
||||
FAIL() << "Something went wrong while executing test.";
|
||||
|
||||
// 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, "diff_drive_fail_test");
|
||||
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
int ret = RUN_ALL_TESTS();
|
||||
spinner.stop();
|
||||
ros::shutdown();
|
||||
return ret;
|
||||
}
|
||||
@@ -0,0 +1,19 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
|
||||
|
||||
<!-- Override with wrong controller configuration -->
|
||||
<rosparam command="load" file="$(find diff_drive_controller)/test/diffbot_wrong.yaml" />
|
||||
<param name="diffbot_controller/wheel_separation" value="0.08"/>
|
||||
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="diff_drive_fail_test"
|
||||
pkg="diff_drive_controller"
|
||||
type="diff_drive_fail_test"
|
||||
time-limit="10.0">
|
||||
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="diffbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,17 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
|
||||
|
||||
<!-- Load diff drive parameter multipliers -->
|
||||
<rosparam command="load" file="$(find diff_drive_controller)/test/diffbot_left_right_multipliers.yaml" />
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="diff_drive_test"
|
||||
pkg="diff_drive_controller"
|
||||
type="diff_drive_test"
|
||||
time-limit="80.0">
|
||||
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="diffbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,231 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// 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 "test_common.h"
|
||||
|
||||
// TEST CASES
|
||||
TEST_F(DiffDriveControllerTest, testLinearJerkLimits)
|
||||
{
|
||||
// wait for ROS
|
||||
waitForController();
|
||||
|
||||
// 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(DiffDriveControllerTest, 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(DiffDriveControllerTest, 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);
|
||||
}
|
||||
|
||||
/* This test has been failing on Travis for a long time due to timing issues however it works well when ran manually
|
||||
TEST_F(DiffDriveControllerTest, 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;
|
||||
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.7rad.s-1
|
||||
EXPECT_NEAR(new_odom.twist.twist.angular.z, 0.7, JERK_ANGULAR_VELOCITY_TOLERANCE);
|
||||
EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), EPS);
|
||||
|
||||
cmd_vel.angular.z = 0.0;
|
||||
publish(cmd_vel);
|
||||
}
|
||||
*/
|
||||
|
||||
TEST_F(DiffDriveControllerTest, 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;
|
||||
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 1.0rad.s-1, which is 2.0rad.s-2 * 0.5s
|
||||
EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), 1.0 + VELOCITY_TOLERANCE);
|
||||
EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), EPS);
|
||||
|
||||
cmd_vel.angular.z = 0.0;
|
||||
publish(cmd_vel);
|
||||
}
|
||||
|
||||
TEST_F(DiffDriveControllerTest, 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();
|
||||
// send a big command
|
||||
cmd_vel.angular.z = 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 2.0rad.s-1, the limit
|
||||
EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), 2.0 + VELOCITY_TOLERANCE);
|
||||
EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), EPS);
|
||||
|
||||
cmd_vel.angular.z = 0.0;
|
||||
publish(cmd_vel);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
ros::init(argc, argv, "diff_drive_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,24 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
|
||||
|
||||
<!-- Override robot_description with sphere wheel urdf -->
|
||||
<param name="robot_description"
|
||||
command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot_sphere_wheels.xacro'" />
|
||||
<!-- Set allow_multiple_publishers to false -->
|
||||
<param name="diffbot_controller/allow_multiple_cmd_vel_publishers" value="false"/>
|
||||
|
||||
<node name="cmd_vel_publisher" pkg="rostopic" type="rostopic" args="pub -r 2 /diffbot_controller/cmd_vel geometry_msgs/Twist '{linear: {x: 1.0}}'" />
|
||||
|
||||
<node name="cmd_vel_publisher2" pkg="rostopic" type="rostopic" args="pub -r 2 /diffbot_controller/cmd_vel geometry_msgs/Twist '{linear: {x: 2.0}}'" />
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="diff_drive_allow_multiple_cmd_vel_publishers_param_test"
|
||||
pkg="diff_drive_controller"
|
||||
type="diff_drive_multiple_cmd_vel_publishers_test"
|
||||
time-limit="80.0">
|
||||
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="diffbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,72 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// 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 "test_common.h"
|
||||
|
||||
// TEST CASES
|
||||
TEST_F(DiffDriveControllerTest, breakWithMultiplePublishers)
|
||||
{
|
||||
// wait for ROS
|
||||
waitForController();
|
||||
waitForOdomMsgs();
|
||||
|
||||
nav_msgs::Odometry old_odom = getLastOdom();
|
||||
//TODO: we should be programatically publish from 2 different nodes
|
||||
// not the current hacky solution with the launch files
|
||||
ros::Duration(1.0).sleep();
|
||||
nav_msgs::Odometry new_odom = getLastOdom();
|
||||
|
||||
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), 0.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);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
ros::init(argc, argv, "diff_drive_multiple_publishers_test");
|
||||
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
int ret = RUN_ALL_TESTS();
|
||||
spinner.stop();
|
||||
ros::shutdown();
|
||||
return ret;
|
||||
}
|
||||
@@ -0,0 +1,17 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
|
||||
|
||||
<!-- Load diff drive parameter multipliers -->
|
||||
<rosparam command="load" file="$(find diff_drive_controller)/test/diffbot_multipliers.yaml" />
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="diff_drive_test"
|
||||
pkg="diff_drive_controller"
|
||||
type="diff_drive_test"
|
||||
time-limit="80.0">
|
||||
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="diffbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,131 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// 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 "test_common.h"
|
||||
|
||||
// NaN
|
||||
#include <limits>
|
||||
|
||||
// TEST CASES
|
||||
TEST_F(DiffDriveControllerTest, testNaN) {
|
||||
// wait for ROS
|
||||
waitForController();
|
||||
|
||||
// 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_FALSE(std::isnan(odom.twist.twist.linear.x));
|
||||
EXPECT_FALSE(std::isnan(odom.twist.twist.angular.z));
|
||||
EXPECT_FALSE(std::isnan(odom.pose.pose.position.x));
|
||||
EXPECT_FALSE(std::isnan(odom.pose.pose.position.y));
|
||||
EXPECT_FALSE(std::isnan(odom.pose.pose.orientation.z));
|
||||
EXPECT_FALSE(std::isnan(odom.pose.pose.orientation.w));
|
||||
|
||||
// start robot
|
||||
start();
|
||||
ros::Duration(2.0).sleep();
|
||||
|
||||
odom = getLastOdom();
|
||||
|
||||
EXPECT_FALSE(std::isnan(odom.twist.twist.linear.x));
|
||||
EXPECT_FALSE(std::isnan(odom.twist.twist.angular.z));
|
||||
EXPECT_FALSE(std::isnan(odom.pose.pose.position.x));
|
||||
EXPECT_FALSE(std::isnan(odom.pose.pose.position.y));
|
||||
EXPECT_FALSE(std::isnan(odom.pose.pose.orientation.z));
|
||||
EXPECT_FALSE(std::isnan(odom.pose.pose.orientation.w));
|
||||
}
|
||||
|
||||
TEST_F(DiffDriveControllerTest, testNaNCmd) {
|
||||
// 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();
|
||||
|
||||
// send NaN
|
||||
for (int i = 0; i < 10; ++i) {
|
||||
geometry_msgs::Twist cmd_vel;
|
||||
cmd_vel.linear.x = NAN;
|
||||
cmd_vel.angular.z = 0.0;
|
||||
publish(cmd_vel);
|
||||
geometry_msgs::TwistStamped odom_msg = getLastCmdVelOut();
|
||||
EXPECT_FALSE(std::isnan(odom_msg.twist.linear.x));
|
||||
EXPECT_FALSE(std::isnan(odom_msg.twist.angular.z));
|
||||
ros::Duration(0.1).sleep();
|
||||
}
|
||||
for (int i = 0; i < 10; ++i) {
|
||||
geometry_msgs::Twist cmd_vel;
|
||||
cmd_vel.linear.x = 0.0;
|
||||
cmd_vel.angular.z = NAN;
|
||||
publish(cmd_vel);
|
||||
geometry_msgs::TwistStamped odom_msg = getLastCmdVelOut();
|
||||
EXPECT_FALSE(std::isnan(odom_msg.twist.linear.x));
|
||||
EXPECT_FALSE(std::isnan(odom_msg.twist.angular.z));
|
||||
ros::Duration(0.1).sleep();
|
||||
}
|
||||
|
||||
nav_msgs::Odometry odom = getLastOdom();
|
||||
EXPECT_DOUBLE_EQ(odom.twist.twist.linear.x, 0.0);
|
||||
EXPECT_DOUBLE_EQ(odom.pose.pose.position.x, 0.0);
|
||||
EXPECT_DOUBLE_EQ(odom.pose.pose.position.y, 0.0);
|
||||
|
||||
geometry_msgs::TwistStamped odom_msg = getLastCmdVelOut();
|
||||
EXPECT_DOUBLE_EQ(odom_msg.twist.linear.x, 0.0);
|
||||
}
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
ros::init(argc, argv, "diff_drive_nan_test");
|
||||
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
int ret = RUN_ALL_TESTS();
|
||||
spinner.stop();
|
||||
ros::shutdown();
|
||||
return ret;
|
||||
}
|
||||
@@ -0,0 +1,20 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
|
||||
|
||||
<!-- Set odom_frame_id to something new -->
|
||||
<rosparam>
|
||||
diffbot_controller:
|
||||
odom_frame_id: new_odom
|
||||
</rosparam>
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="diff_drive_odom_frame_test"
|
||||
pkg="diff_drive_controller"
|
||||
type="diff_drive_odom_frame_test"
|
||||
time-limit="80.0">
|
||||
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="diffbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,83 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// 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 "test_common.h"
|
||||
#include <tf/transform_listener.h>
|
||||
|
||||
// TEST CASES
|
||||
TEST_F(DiffDriveControllerTest, testNoOdomFrame)
|
||||
{
|
||||
// wait for ROS
|
||||
waitForController();
|
||||
|
||||
// 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(DiffDriveControllerTest, testNewOdomFrame)
|
||||
{
|
||||
// wait for ROS
|
||||
waitForController();
|
||||
|
||||
// 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(DiffDriveControllerTest, testOdomTopic)
|
||||
{
|
||||
// wait for ROS
|
||||
waitForController();
|
||||
|
||||
// wait until we received first odom msg
|
||||
waitForOdomMsgs();
|
||||
|
||||
// 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, "diff_drive_odom_frame_test");
|
||||
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
int ret = RUN_ALL_TESTS();
|
||||
spinner.stop();
|
||||
ros::shutdown();
|
||||
return ret;
|
||||
}
|
||||
@@ -0,0 +1,20 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
|
||||
|
||||
<!-- Make sure to disable odom tf -->
|
||||
<rosparam>
|
||||
diffbot_controller:
|
||||
enable_odom_tf: False
|
||||
</rosparam>
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="diff_drive_odom_tf_test"
|
||||
pkg="diff_drive_controller"
|
||||
type="diff_drive_odom_tf_test"
|
||||
time-limit="80.0">
|
||||
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="diffbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,57 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// 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 "test_common.h"
|
||||
#include <tf/transform_listener.h>
|
||||
|
||||
// TEST CASES
|
||||
TEST_F(DiffDriveControllerTest, testNoOdomFrame)
|
||||
{
|
||||
// wait for ROS
|
||||
waitForController();
|
||||
|
||||
// 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, "diff_drive_odom_tf_test");
|
||||
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
int ret = RUN_ALL_TESTS();
|
||||
spinner.stop();
|
||||
ros::shutdown();
|
||||
return ret;
|
||||
}
|
||||
@@ -0,0 +1,17 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
|
||||
|
||||
<!-- Load diff drive parameter open loop -->
|
||||
<rosparam command="load" file="$(find diff_drive_controller)/test/diffbot_open_loop.yaml" />
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="diff_drive_test"
|
||||
pkg="diff_drive_controller"
|
||||
type="diff_drive_test"
|
||||
time-limit="80.0">
|
||||
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="diffbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,20 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
|
||||
|
||||
<rosparam>
|
||||
diffbot_controller:
|
||||
publish_cmd: True
|
||||
</rosparam>
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="diff_drive_pub_cmd_vel_out_test"
|
||||
pkg="diff_drive_controller"
|
||||
type="diff_drive_pub_cmd_vel_out_test"
|
||||
time-limit="80.0">
|
||||
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="diffbot_controller/odom" />
|
||||
<remap from="cmd_vel_out" to="diffbot_controller/cmd_vel_out" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,74 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// Copyright (C) 2017, PAL Robotics.
|
||||
//
|
||||
// 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 Jeremie Deray
|
||||
|
||||
#include "test_common.h"
|
||||
|
||||
// TEST CASES
|
||||
TEST_F(DiffDriveControllerTest, testCmdVelOutTopic)
|
||||
{
|
||||
// wait for ROS
|
||||
waitForController();
|
||||
// msgs are published in the same loop
|
||||
// thus if odom is published cmd_vel_out
|
||||
// should be as well (if enabled)
|
||||
waitForOdomMsgs();
|
||||
|
||||
EXPECT_TRUE(isPublishingCmdVelOut());
|
||||
|
||||
// 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();
|
||||
|
||||
cmd_vel.linear.x = 0.1;
|
||||
publish(cmd_vel);
|
||||
ros::Duration(0.1).sleep();
|
||||
|
||||
EXPECT_TRUE(isPublishingCmdVelOut());
|
||||
|
||||
// get a twist message
|
||||
geometry_msgs::TwistStamped odom_msg = getLastCmdVelOut();
|
||||
|
||||
EXPECT_GT(fabs(odom_msg.twist.linear.x), 0);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
ros::init(argc, argv, "diff_drive_pub_cmd_vel_out_topic_test");
|
||||
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
int ret = RUN_ALL_TESTS();
|
||||
spinner.stop();
|
||||
ros::shutdown();
|
||||
return ret;
|
||||
}
|
||||
@@ -0,0 +1,21 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
|
||||
|
||||
<!-- default value -->
|
||||
<rosparam>
|
||||
diffbot_controller:
|
||||
publish_wheel_joint_controller_state: True
|
||||
</rosparam>
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="diff_drive_publish_wheel_joint_controller_state__test"
|
||||
pkg="diff_drive_controller"
|
||||
type="diff_drive_publish_wheel_joint_controller_state_test"
|
||||
time-limit="30.0">
|
||||
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="diffbot_controller/odom" />
|
||||
<remap from="wheel_joint_controller_state" to="diffbot_controller/wheel_joint_controller_state" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,65 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// Copyright (C) 2018, PAL Robotics.
|
||||
//
|
||||
// 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 Jeremie Deray
|
||||
|
||||
#include "test_common.h"
|
||||
|
||||
// TEST CASES
|
||||
TEST_F(DiffDriveControllerTest, testPublishJointTrajectoryControllerStateTopic)
|
||||
{
|
||||
// wait for ROS
|
||||
waitForController();
|
||||
|
||||
EXPECT_TRUE(isPublishingJointTrajectoryControllerState());
|
||||
|
||||
// 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();
|
||||
|
||||
cmd_vel.linear.x = 0.1;
|
||||
publish(cmd_vel);
|
||||
ros::Duration(0.1).sleep();
|
||||
|
||||
EXPECT_TRUE(isPublishingJointTrajectoryControllerState());
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
ros::init(argc, argv, "diff_drive_publish_wheel_joint_controller_state_topic_test");
|
||||
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
int ret = RUN_ALL_TESTS();
|
||||
spinner.stop();
|
||||
ros::shutdown();
|
||||
return ret;
|
||||
}
|
||||
@@ -0,0 +1,20 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
|
||||
|
||||
<!-- Override robot_description with square wheel urdf -->
|
||||
<param name="robot_description"
|
||||
command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot_square_wheels.xacro'" />
|
||||
<!-- Provide the radius, since the bot's wheels are boxes, not cylinders or spheres -->
|
||||
<param name="diffbot_controller/wheel_radius" value="0.11"/>
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="diff_drive_wheel_radius_param_test"
|
||||
pkg="diff_drive_controller"
|
||||
type="diff_drive_test"
|
||||
time-limit="80.0">
|
||||
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="diffbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,20 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
|
||||
|
||||
<!-- Override robot_description with bad urdf -->
|
||||
<param name="robot_description"
|
||||
command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot_square_wheels.xacro'" />
|
||||
<!-- 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="diff_drive_wheel_radius_param_fail_test"
|
||||
pkg="diff_drive_controller"
|
||||
type="diff_drive_fail_test"
|
||||
time-limit="10.0">
|
||||
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="diffbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,20 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
|
||||
|
||||
<!-- Override robot_description with sphere wheel urdf -->
|
||||
<param name="robot_description"
|
||||
command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot_sphere_wheels.xacro'" />
|
||||
<!-- Don't provide the radius, since controller should accept cylinders *or* spheres -->
|
||||
<!-- param name="diffbot_controller/wheel_radius" value="0.11"/ -->
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="diff_drive_wheel_radius_param_test"
|
||||
pkg="diff_drive_controller"
|
||||
type="diff_drive_test"
|
||||
time-limit="80.0">
|
||||
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="diffbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,22 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
|
||||
|
||||
<!-- Override robot_description with sphere wheel urdf -->
|
||||
<param name="robot_description"
|
||||
command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot_sphere_wheels.xacro'" />
|
||||
<!-- Provide the radius, since the bot's wheels are spheres, not cylinders -->
|
||||
<param name="diffbot_controller/wheel_radius" value="0.11"/>
|
||||
<!-- Provide the wheel separation -->
|
||||
<param name="diffbot_controller/wheel_separation" value="1.0"/>
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="diff_drive_wheel_separation_param_test"
|
||||
pkg="diff_drive_controller"
|
||||
type="diff_drive_test"
|
||||
time-limit="80.0">
|
||||
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="diffbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,154 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// 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 "test_common.h"
|
||||
#include <tf/transform_listener.h>
|
||||
|
||||
// TEST CASES
|
||||
TEST_F(DiffDriveControllerTest, testForward)
|
||||
{
|
||||
// wait for ROS
|
||||
waitForController();
|
||||
|
||||
// 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 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();
|
||||
|
||||
const ros::Duration actual_elapsed_time = new_odom.header.stamp - old_odom.header.stamp;
|
||||
|
||||
const double expected_distance = cmd_vel.linear.x * actual_elapsed_time.toSec();
|
||||
|
||||
// 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), expected_distance, 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(DiffDriveControllerTest, testTurn)
|
||||
{
|
||||
// wait for ROS
|
||||
waitForController();
|
||||
|
||||
// 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 velocity command
|
||||
cmd_vel.angular.z = M_PI/10.0;
|
||||
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 the other fields should be ~~0
|
||||
EXPECT_LT(fabs(new_odom.pose.pose.position.x - old_odom.pose.pose.position.x), EPS);
|
||||
EXPECT_LT(fabs(new_odom.pose.pose.position.y - old_odom.pose.pose.position.y), EPS);
|
||||
EXPECT_LT(fabs(new_odom.pose.pose.position.z - old_odom.pose.pose.position.z), EPS);
|
||||
|
||||
const ros::Duration actual_elapsed_time = new_odom.header.stamp - old_odom.header.stamp;
|
||||
const double expected_rotation = cmd_vel.angular.z * actual_elapsed_time.toSec();
|
||||
|
||||
// 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), expected_rotation, ORIENTATION_TOLERANCE);
|
||||
|
||||
EXPECT_LT(fabs(new_odom.twist.twist.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_NEAR(fabs(new_odom.twist.twist.angular.z), expected_rotation/actual_elapsed_time.toSec(), EPS);
|
||||
}
|
||||
|
||||
TEST_F(DiffDriveControllerTest, testOdomFrame)
|
||||
{
|
||||
// wait for ROS
|
||||
waitForController();
|
||||
|
||||
// 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, "diff_drive_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,17 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
|
||||
|
||||
<!-- Load diff-drive cmd_vel_timeout -->
|
||||
<rosparam command="load" file="$(find diff_drive_controller)/test/diffbot_timeout.yaml" />
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="diff_drive_timeout_test"
|
||||
pkg="diff_drive_controller"
|
||||
type="diff_drive_timeout_test"
|
||||
time-limit="20.0">
|
||||
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="diffbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,70 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// 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 "test_common.h"
|
||||
|
||||
// TEST CASES
|
||||
TEST_F(DiffDriveControllerTest, testTimeout)
|
||||
{
|
||||
// wait for ROS
|
||||
waitForController();
|
||||
|
||||
// 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, "diff_drive_test");
|
||||
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
int ret = RUN_ALL_TESTS();
|
||||
spinner.stop();
|
||||
ros::shutdown();
|
||||
return ret;
|
||||
}
|
||||
@@ -0,0 +1,17 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
|
||||
|
||||
<!-- Override with wrong controller configuration -->
|
||||
<rosparam command="load" file="$(find diff_drive_controller)/test/diffbot_wrong.yaml" />
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="diff_drive_fail_test"
|
||||
pkg="diff_drive_controller"
|
||||
type="diff_drive_fail_test"
|
||||
time-limit="10.0">
|
||||
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="diffbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,93 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// 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
|
||||
|
||||
#include "diffbot.h"
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
#include <controller_manager/controller_manager.h>
|
||||
#include <ros/ros.h>
|
||||
#include <rosgraph_msgs/Clock.h>
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "diffbot");
|
||||
ros::NodeHandle nh;
|
||||
|
||||
// This should be set in launch files as well
|
||||
nh.setParam("/use_sim_time", true);
|
||||
|
||||
Diffbot<> robot;
|
||||
ROS_WARN_STREAM("period: " << robot.getPeriod().toSec());
|
||||
controller_manager::ControllerManager cm(&robot, nh);
|
||||
|
||||
ros::Publisher clock_publisher = nh.advertise<rosgraph_msgs::Clock>("/clock", 1);
|
||||
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
|
||||
std::chrono::system_clock::time_point begin = std::chrono::system_clock::now();
|
||||
std::chrono::system_clock::time_point end = std::chrono::system_clock::now();
|
||||
|
||||
ros::Time internal_time(0);
|
||||
const ros::Duration dt = robot.getPeriod();
|
||||
double elapsed_secs = 0;
|
||||
|
||||
while(ros::ok())
|
||||
{
|
||||
begin = std::chrono::system_clock::now();
|
||||
|
||||
robot.read();
|
||||
cm.update(internal_time, dt);
|
||||
|
||||
robot.write();
|
||||
|
||||
end = std::chrono::system_clock::now();
|
||||
|
||||
elapsed_secs = std::chrono::duration_cast<std::chrono::duration<double> >((end - begin)).count();
|
||||
|
||||
if (dt.toSec() - elapsed_secs < 0.0)
|
||||
{
|
||||
ROS_WARN_STREAM_THROTTLE(
|
||||
0.1, "Control cycle is taking to much time, elapsed: " << elapsed_secs);
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_DEBUG_STREAM_THROTTLE(1.0, "Control cycle is, elapsed: " << elapsed_secs);
|
||||
std::this_thread::sleep_for(std::chrono::duration<double>(dt.toSec() - elapsed_secs));
|
||||
}
|
||||
|
||||
rosgraph_msgs::Clock clock;
|
||||
clock.clock = ros::Time(internal_time);
|
||||
clock_publisher.publish(clock);
|
||||
internal_time += dt;
|
||||
}
|
||||
spinner.stop();
|
||||
|
||||
return 0;
|
||||
}
|
||||
143
Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot.h
Executable file
143
Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot.h
Executable file
@@ -0,0 +1,143 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// 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
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
// ROS
|
||||
#include <ros/ros.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>
|
||||
|
||||
template <unsigned int NUM_JOINTS = 2>
|
||||
class Diffbot : public hardware_interface::RobotHW
|
||||
{
|
||||
public:
|
||||
Diffbot()
|
||||
: running_(true)
|
||||
, start_srv_(nh_.advertiseService("start", &Diffbot::start_callback, this))
|
||||
, stop_srv_(nh_.advertiseService("stop", &Diffbot::stop_callback, this))
|
||||
{
|
||||
// Intialize raw data
|
||||
std::fill_n(pos_, NUM_JOINTS, 0.0);
|
||||
std::fill_n(vel_, NUM_JOINTS, 0.0);
|
||||
std::fill_n(eff_, NUM_JOINTS, 0.0);
|
||||
std::fill_n(cmd_, NUM_JOINTS, 0.0);
|
||||
|
||||
// Connect and register the joint state and velocity interface
|
||||
for (unsigned int i = 0; i < NUM_JOINTS; ++i)
|
||||
{
|
||||
std::ostringstream os;
|
||||
os << "wheel_" << i << "_joint";
|
||||
|
||||
hardware_interface::JointStateHandle state_handle(os.str(), &pos_[i], &vel_[i], &eff_[i]);
|
||||
jnt_state_interface_.registerHandle(state_handle);
|
||||
|
||||
hardware_interface::JointHandle vel_handle(jnt_state_interface_.getHandle(os.str()), &cmd_[i]);
|
||||
jnt_vel_interface_.registerHandle(vel_handle);
|
||||
}
|
||||
|
||||
registerInterface(&jnt_state_interface_);
|
||||
registerInterface(&jnt_vel_interface_);
|
||||
}
|
||||
|
||||
ros::Time getTime() const {return ros::Time::now();}
|
||||
ros::Duration getPeriod() const {return ros::Duration(0.01);}
|
||||
|
||||
void read()
|
||||
{
|
||||
// Read the joint state of the robot into the hardware interface
|
||||
if (running_)
|
||||
{
|
||||
for (unsigned int i = 0; i < NUM_JOINTS; ++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.
|
||||
pos_[i] += vel_[i]*getPeriod().toSec(); // update position
|
||||
vel_[i] = cmd_[i]; // might add smoothing here later
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
std::fill_n(pos_, NUM_JOINTS, std::numeric_limits<double>::quiet_NaN());
|
||||
std::fill_n(vel_, NUM_JOINTS, std::numeric_limits<double>::quiet_NaN());
|
||||
}
|
||||
}
|
||||
|
||||
void write()
|
||||
{
|
||||
// Write the commands to the joints
|
||||
std::ostringstream os;
|
||||
for (unsigned int i = 0; i < NUM_JOINTS - 1; ++i)
|
||||
{
|
||||
os << cmd_[i] << ", ";
|
||||
}
|
||||
os << cmd_[NUM_JOINTS - 1];
|
||||
|
||||
ROS_INFO_STREAM("Commands for joints: " << os.str());
|
||||
}
|
||||
|
||||
bool start_callback(std_srvs::Empty::Request& /*req*/, std_srvs::Empty::Response& /*res*/)
|
||||
{
|
||||
running_ = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool stop_callback(std_srvs::Empty::Request& /*req*/, std_srvs::Empty::Response& /*res*/)
|
||||
{
|
||||
running_ = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
private:
|
||||
hardware_interface::JointStateInterface jnt_state_interface_;
|
||||
hardware_interface::VelocityJointInterface jnt_vel_interface_;
|
||||
double cmd_[NUM_JOINTS];
|
||||
double pos_[NUM_JOINTS];
|
||||
double vel_[NUM_JOINTS];
|
||||
double eff_[NUM_JOINTS];
|
||||
bool running_;
|
||||
|
||||
ros::NodeHandle nh_;
|
||||
ros::ServiceServer start_srv_;
|
||||
ros::ServiceServer stop_srv_;
|
||||
};
|
||||
@@ -0,0 +1,77 @@
|
||||
<?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 diff_drive_controller)/test/wheel.xacro"/>
|
||||
|
||||
<xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
|
||||
|
||||
<!-- Constants for robot dimensions -->
|
||||
<xacro:property name="width" value="0.5" /> <!-- Square dimensions (widthxwidth) of beams -->
|
||||
<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 -->
|
||||
|
||||
<!-- 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="${width} 0.1 0.1"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<!-- origin is relative -->
|
||||
<origin xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="${width} 0.1 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>
|
||||
|
||||
|
||||
<!-- Wheels -->
|
||||
<xacro:wheel name="wheel_0" parent="base" radius="${wheel_radius}" thickness="${thickness}">
|
||||
<origin xyz="${width/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
|
||||
</xacro:wheel>
|
||||
<xacro:wheel name="wheel_1" parent="base" radius="${wheel_radius}" thickness="${thickness}">
|
||||
<origin xyz="${-width/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
|
||||
</xacro:wheel>
|
||||
|
||||
|
||||
<!-- Colour -->
|
||||
<gazebo reference="base_link">
|
||||
<material>Gazebo/Green</material>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="base_footprint">
|
||||
<material>Gazebo/Purple</material>
|
||||
</gazebo>
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,142 @@
|
||||
<?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
|
||||
-->
|
||||
<!-- Constants for robot dimensions -->
|
||||
<xacro:property name="PI" value="3.1415926535897931"/>
|
||||
<xacro:property name="width" value="0.5" /> <!-- Square dimensions (widthxwidth) of beams -->
|
||||
<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 -->
|
||||
|
||||
<!-- 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="${width} 0.1 0.1"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<!-- origin is relative -->
|
||||
<origin xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="${width} 0.1 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>
|
||||
|
||||
|
||||
<link name="wheel1">
|
||||
<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 ${PI/2} 0"/>
|
||||
<geometry>
|
||||
<sphere radius="${wheel_radius}"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 ${PI/2} 0"/>
|
||||
<geometry>
|
||||
<sphere radius="${wheel_radius}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint_w1" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="wheel1"/>
|
||||
<origin xyz="${width/2+axel_offset} 0 0" rpy="${-PI/2} 0 0"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="wheel2">
|
||||
<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 ${PI/2} 0"/>
|
||||
<geometry>
|
||||
<sphere radius="${wheel_radius}"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 ${PI/2} 0"/>
|
||||
<geometry>
|
||||
<sphere radius="${wheel_radius}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint_w2" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="wheel2"/>
|
||||
<origin xyz="${-width/2-axel_offset} 0 0" rpy="${-PI/2} 0 0"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<!-- Transmission is important to link the joints and the controller -->
|
||||
<transmission name="joint_w1_trans" type="SimpleTransmission">
|
||||
<actuator name="joint_w1_motor" />
|
||||
<joint name="joint_w1" />
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<motorTorqueConstant>1</motorTorqueConstant>
|
||||
</transmission>
|
||||
|
||||
<transmission name="joint_w2_trans" type="SimpleTransmission">
|
||||
<actuator name="joint_w2_motor" />
|
||||
<joint name="joint_w2" />
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<motorTorqueConstant>1</motorTorqueConstant>
|
||||
</transmission>
|
||||
|
||||
<!-- Colour -->
|
||||
<gazebo reference="base_link">
|
||||
<material>Gazebo/Green</material>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="wheel1">
|
||||
<material>Gazebo/Red</material>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="wheel2">
|
||||
<material>Gazebo/Blue</material>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="base_footprint">
|
||||
<material>Gazebo/Purple</material>
|
||||
</gazebo>
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,8 @@
|
||||
diffbot_controller:
|
||||
type: "diff_drive_controller/DiffDriveController"
|
||||
left_wheel: 'wheel_0_joint'
|
||||
right_wheel: 'wheel_1_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,4 @@
|
||||
diffbot_controller:
|
||||
wheel_separation_multiplier: 2.3
|
||||
left_wheel_radius_multiplier: 1.4
|
||||
right_wheel_radius_multiplier: 1.4
|
||||
@@ -0,0 +1,19 @@
|
||||
diffbot_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: 2.0
|
||||
has_acceleration_limits: true
|
||||
max_acceleration: 2.0
|
||||
has_jerk_limits: true
|
||||
max_jerk: 10.0
|
||||
@@ -0,0 +1,3 @@
|
||||
diffbot_controller:
|
||||
wheel_separation_multiplier: 2.3
|
||||
wheel_radius_multiplier: 1.4
|
||||
@@ -0,0 +1,2 @@
|
||||
diffbot_controller:
|
||||
open_loop: true
|
||||
@@ -0,0 +1,75 @@
|
||||
<?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 diff_drive_controller)/test/sphere_wheel.xacro"/>
|
||||
<xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
|
||||
|
||||
<!-- Constants for robot dimensions -->
|
||||
<xacro:property name="width" value="0.5" /> <!-- Square dimensions (widthxwidth) of beams -->
|
||||
<xacro:property name="wheel_radius" value="0.11" /> <!-- Link 1 -->
|
||||
<xacro:property name="axel_offset" value="0.05" /> <!-- Space btw top of beam and the each joint -->
|
||||
|
||||
<!-- 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="${width} 0.1 0.1"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<!-- origin is relative -->
|
||||
<origin xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="${width} 0.1 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>
|
||||
|
||||
|
||||
<!-- Wheels -->
|
||||
<xacro:sphere_wheel name="wheel_0" parent="base" radius="${wheel_radius}">
|
||||
<origin xyz="${width/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
|
||||
</xacro:sphere_wheel>
|
||||
<xacro:sphere_wheel name="wheel_1" parent="base" radius="${wheel_radius}">
|
||||
<origin xyz="${-width/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
|
||||
</xacro:sphere_wheel>
|
||||
|
||||
|
||||
<!-- Colour -->
|
||||
<gazebo reference="base_link">
|
||||
<material>Gazebo/Green</material>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="base_footprint">
|
||||
<material>Gazebo/Purple</material>
|
||||
</gazebo>
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,75 @@
|
||||
<?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 diff_drive_controller)/test/square_wheel.xacro"/>
|
||||
<xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
|
||||
|
||||
<!-- Constants for robot dimensions -->
|
||||
<xacro:property name="width" value="0.5" /> <!-- Square dimensions (widthxwidth) of beams -->
|
||||
<xacro:property name="wheel_radius" value="0.11" /> <!-- Link 1 -->
|
||||
<xacro:property name="axel_offset" value="0.05" /> <!-- Space btw top of beam and the each joint -->
|
||||
|
||||
<!-- 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="${width} 0.1 0.1"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<!-- origin is relative -->
|
||||
<origin xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="${width} 0.1 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>
|
||||
|
||||
|
||||
<!-- Wheels -->
|
||||
<xacro:square_wheel name="wheel_0" parent="base" radius="${wheel_radius}">
|
||||
<origin xyz="${width/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
|
||||
</xacro:square_wheel>
|
||||
<xacro:square_wheel name="wheel_1" parent="base" radius="${wheel_radius}">
|
||||
<origin xyz="${-width/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
|
||||
</xacro:square_wheel>
|
||||
|
||||
|
||||
<!-- Colour -->
|
||||
<gazebo reference="base_link">
|
||||
<material>Gazebo/Green</material>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="base_footprint">
|
||||
<material>Gazebo/Purple</material>
|
||||
</gazebo>
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,2 @@
|
||||
diffbot_controller:
|
||||
cmd_vel_timeout: 0.5
|
||||
@@ -0,0 +1,8 @@
|
||||
diffbot_controller:
|
||||
type: "diff_drive_controller/DiffDriveController"
|
||||
left_wheel: 'this_joint_does_not_exist'
|
||||
right_wheel: 'right_wheel_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,35 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Use simulation time -->
|
||||
<rosparam param="use_sim_time">True</rosparam>
|
||||
|
||||
<!-- Load skidsteerbot model -->
|
||||
<param name="robot_description"
|
||||
command="$(find xacro)/xacro '$(find diff_drive_controller)/test/skidsteerbot.xacro'" />
|
||||
|
||||
<!-- Start skidsteerbot -->
|
||||
<node name="skidsteerbot"
|
||||
pkg="diff_drive_controller"
|
||||
type="skidsteerbot"/>
|
||||
|
||||
<!-- Load controller config -->
|
||||
<rosparam command="load" file="$(find diff_drive_controller)/test/skidsteerbot_controllers.yaml" />
|
||||
|
||||
<!-- Spawn controller -->
|
||||
<node name="controller_spawner"
|
||||
pkg="controller_manager" type="spawner" output="screen"
|
||||
args="skidsteerbot_controller" />
|
||||
|
||||
<!-- rqt_plot monitoring -->
|
||||
<!--
|
||||
<node name="skidsteerbot_pos_monitor"
|
||||
pkg="rqt_plot"
|
||||
type="rqt_plot"
|
||||
args="/skidsteerbot_controller/odom/pose/pose/position/x" />
|
||||
|
||||
<node name="skidsteerbot_vel_monitor"
|
||||
pkg="rqt_plot"
|
||||
type="rqt_plot"
|
||||
args="/skidsteerbot_controller/odom/twist/twist/linear/x" />
|
||||
-->
|
||||
</launch>
|
||||
@@ -0,0 +1,14 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find diff_drive_controller)/test/skid_steer_common.launch" />
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="skid_steer_test"
|
||||
pkg="diff_drive_controller"
|
||||
type="diff_drive_test"
|
||||
time-limit="80.0">
|
||||
<remap from="cmd_vel" to="skidsteerbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="skidsteerbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,17 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Load common test stuff -->
|
||||
<include file="$(find diff_drive_controller)/test/skid_steer_common.launch" />
|
||||
|
||||
<!-- Override with wrong controller configuration -->
|
||||
<rosparam command="load" file="$(find diff_drive_controller)/test/skidsteerbot_no_wheels.yaml" />
|
||||
|
||||
<!-- Controller test -->
|
||||
<test test-name="skid_steer_fail_test"
|
||||
pkg="diff_drive_controller"
|
||||
type="diff_drive_fail_test"
|
||||
time-limit="10.0">
|
||||
<remap from="cmd_vel" to="skidsteerbot_controller/cmd_vel" />
|
||||
<remap from="odom" to="skidsteerbot_controller/odom" />
|
||||
</test>
|
||||
</launch>
|
||||
@@ -0,0 +1,93 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// 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
|
||||
|
||||
#include "diffbot.h"
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
#include <controller_manager/controller_manager.h>
|
||||
#include <ros/ros.h>
|
||||
#include <rosgraph_msgs/Clock.h>
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "skidsteerbot");
|
||||
ros::NodeHandle nh;
|
||||
|
||||
// This should be set in launch files as well
|
||||
nh.setParam("/use_sim_time", true);
|
||||
|
||||
Diffbot<6> robot;
|
||||
ROS_WARN_STREAM("period: " << robot.getPeriod().toSec());
|
||||
controller_manager::ControllerManager cm(&robot, nh);
|
||||
|
||||
ros::Publisher clock_publisher = nh.advertise<rosgraph_msgs::Clock>("/clock", 1);
|
||||
|
||||
//ros::Rate rate(1.0 / robot.getPeriod().toSec());
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
|
||||
std::chrono::system_clock::time_point begin = std::chrono::system_clock::now();
|
||||
std::chrono::system_clock::time_point end = std::chrono::system_clock::now();
|
||||
|
||||
ros::Time internal_time(0);
|
||||
const ros::Duration dt = robot.getPeriod();
|
||||
double elapsed_secs = 0;
|
||||
|
||||
while(ros::ok())
|
||||
{
|
||||
begin = std::chrono::system_clock::now();
|
||||
|
||||
robot.read();
|
||||
cm.update(internal_time, dt);
|
||||
robot.write();
|
||||
|
||||
end = std::chrono::system_clock::now();
|
||||
|
||||
elapsed_secs = std::chrono::duration_cast<std::chrono::duration<double> >((end - begin)).count();
|
||||
|
||||
if (dt.toSec() - elapsed_secs < 0.0)
|
||||
{
|
||||
ROS_WARN_STREAM_THROTTLE(
|
||||
0.1, "Control cycle is taking to much time, elapsed: " << elapsed_secs);
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_DEBUG_STREAM_THROTTLE(1.0, "Control cycle is, elapsed: " << elapsed_secs);
|
||||
std::this_thread::sleep_for(std::chrono::duration<double>(dt.toSec() - elapsed_secs));
|
||||
}
|
||||
|
||||
rosgraph_msgs::Clock clock;
|
||||
clock.clock = ros::Time(internal_time);
|
||||
clock_publisher.publish(clock);
|
||||
internal_time += dt;
|
||||
}
|
||||
spinner.stop();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,90 @@
|
||||
<?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 diff_drive_controller)/test/wheel.xacro"/>
|
||||
|
||||
<xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
|
||||
|
||||
<!-- Constants for robot dimensions -->
|
||||
<xacro:property name="width" value="0.5" /> <!-- Square dimensions (widthxwidth) of beams -->
|
||||
<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="y_offset" value="0.35" /> <!-- Offset for the wheels on the same side -->
|
||||
|
||||
<!-- 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="${width} 0.1 0.1"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<!-- origin is relative -->
|
||||
<origin xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="${width} 0.1 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>
|
||||
|
||||
|
||||
<!-- Wheels -->
|
||||
<xacro:wheel name="wheel_0" parent="base" radius="${wheel_radius}" thickness="${thickness}">
|
||||
<origin xyz="${width/2+axel_offset} ${y_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
|
||||
</xacro:wheel>
|
||||
<xacro:wheel name="wheel_1" parent="base" radius="${wheel_radius}" thickness="${thickness}">
|
||||
<origin xyz="${width/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
|
||||
</xacro:wheel>
|
||||
<xacro:wheel name="wheel_2" parent="base" radius="${wheel_radius}" thickness="${thickness}">
|
||||
<origin xyz="${width/2+axel_offset} ${-y_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
|
||||
</xacro:wheel>
|
||||
<xacro:wheel name="wheel_3" parent="base" radius="${wheel_radius}" thickness="${thickness}">
|
||||
<origin xyz="${-width/2+axel_offset} ${y_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
|
||||
</xacro:wheel>
|
||||
<xacro:wheel name="wheel_4" parent="base" radius="${wheel_radius}" thickness="${thickness}">
|
||||
<origin xyz="${-width/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
|
||||
</xacro:wheel>
|
||||
<xacro:wheel name="wheel_5" parent="base" radius="${wheel_radius}" thickness="${thickness}">
|
||||
<origin xyz="${-width/2+axel_offset} ${-y_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
|
||||
</xacro:wheel>
|
||||
|
||||
|
||||
<!-- Colour -->
|
||||
<gazebo reference="base_link">
|
||||
<material>Gazebo/Green</material>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="base_footprint">
|
||||
<material>Gazebo/Purple</material>
|
||||
</gazebo>
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,8 @@
|
||||
skidsteerbot_controller:
|
||||
type: "diff_drive_controller/DiffDriveController"
|
||||
left_wheel: ['wheel_0_joint', 'wheel_1_joint', 'wheel_2_joint']
|
||||
right_wheel: ['wheel_3_joint', 'wheel_4_joint', 'wheel_5_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,8 @@
|
||||
skidsteerbot_controller:
|
||||
type: "diff_drive_controller/DiffDriveController"
|
||||
left_wheel: []
|
||||
right_wheel: ['wheel_3_joint', 'wheel_4_joint', 'wheel_5_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,43 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<xacro:macro name="sphere_wheel" params="name parent radius *origin">
|
||||
<link name="${name}_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}_joint" type="continuous">
|
||||
<parent link="${parent}_link"/>
|
||||
<child link="${name}_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}_joint_trans" type="SimpleTransmission">
|
||||
<actuator name="${name}_joint_motor"/>
|
||||
<joint name="${name}_joint"/>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<motorTorqueConstant>1</motorTorqueConstant>
|
||||
</transmission>
|
||||
|
||||
<gazebo reference="${name}_link">
|
||||
<material>Gazebo/Red</material>
|
||||
</gazebo>
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
@@ -0,0 +1,44 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<xacro:macro name="square_wheel" params="name parent radius *origin">
|
||||
<link name="${name}_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>
|
||||
<!-- use boxes for wheels, scale radius to account for "circumference" difference -->
|
||||
<box size="${wheel_radius*1.11} ${wheel_radius*1.11} ${wheel_radius*1.11} "/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="${wheel_radius*1.11} ${wheel_radius*1.11} ${wheel_radius*1.11} "/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="${name}_joint" type="continuous">
|
||||
<parent link="${parent}_link"/>
|
||||
<child link="${name}_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}_joint_trans" type="SimpleTransmission">
|
||||
<actuator name="${name}_joint_motor"/>
|
||||
<joint name="${name}_joint"/>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<motorTorqueConstant>1</motorTorqueConstant>
|
||||
</transmission>
|
||||
|
||||
<gazebo reference="${name}_link">
|
||||
<material>Gazebo/Red</material>
|
||||
</gazebo>
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
@@ -0,0 +1,162 @@
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// 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
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
|
||||
#include <geometry_msgs/TwistStamped.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <control_msgs/JointTrajectoryControllerState.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 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 DiffDriveControllerTest : public ::testing::Test
|
||||
{
|
||||
public:
|
||||
|
||||
DiffDriveControllerTest()
|
||||
: received_first_odom(false)
|
||||
, cmd_pub(nh.advertise<geometry_msgs::Twist>("cmd_vel", 100))
|
||||
, odom_sub(nh.subscribe("odom", 100, &DiffDriveControllerTest::odomCallback, this))
|
||||
, vel_out_sub(nh.subscribe("cmd_vel_out", 100, &DiffDriveControllerTest::cmdVelOutCallback, this))
|
||||
, joint_traj_controller_state_sub(nh.subscribe("wheel_joint_controller_state", 100, &DiffDriveControllerTest::jointTrajectoryControllerStateCallback, this))
|
||||
, start_srv(nh.serviceClient<std_srvs::Empty>("start"))
|
||||
, stop_srv(nh.serviceClient<std_srvs::Empty>("stop"))
|
||||
{
|
||||
}
|
||||
|
||||
~DiffDriveControllerTest()
|
||||
{
|
||||
odom_sub.shutdown();
|
||||
joint_traj_controller_state_sub.shutdown();
|
||||
}
|
||||
|
||||
nav_msgs::Odometry getLastOdom(){ return last_odom; }
|
||||
geometry_msgs::TwistStamped getLastCmdVelOut(){ return last_cmd_vel_out; }
|
||||
control_msgs::JointTrajectoryControllerState getLastJointTrajectoryControllerState(){ return last_joint_traj_controller_state; }
|
||||
void publish(geometry_msgs::Twist cmd_vel){ cmd_pub.publish(cmd_vel); }
|
||||
bool isControllerAlive()const{ return (odom_sub.getNumPublishers() > 0); }
|
||||
bool isPublishingCmdVelOut(const ros::Duration &timeout=ros::Duration(1)) const
|
||||
{
|
||||
ros::Time start = ros::Time::now();
|
||||
int get_num_publishers = vel_out_sub.getNumPublishers();
|
||||
while ( (get_num_publishers == 0) && (ros::Time::now() < start + timeout) ) {
|
||||
ros::Duration(0.1).sleep();
|
||||
get_num_publishers = vel_out_sub.getNumPublishers();
|
||||
}
|
||||
return (get_num_publishers > 0);
|
||||
}
|
||||
bool isPublishingJointTrajectoryControllerState(){ return (joint_traj_controller_state_sub.getNumPublishers() > 0); }
|
||||
bool hasReceivedFirstOdom()const{ return received_first_odom; }
|
||||
|
||||
void start(){ std_srvs::Empty srv; start_srv.call(srv); }
|
||||
void stop(){ std_srvs::Empty srv; stop_srv.call(srv); }
|
||||
|
||||
void waitForController() const
|
||||
{
|
||||
while(!isControllerAlive() && ros::ok())
|
||||
{
|
||||
ROS_DEBUG_STREAM_THROTTLE(0.5, "Waiting for controller.");
|
||||
ros::Duration(0.1).sleep();
|
||||
}
|
||||
if (!ros::ok())
|
||||
FAIL() << "Something went wrong while executing test.";
|
||||
}
|
||||
|
||||
void waitForOdomMsgs() const
|
||||
{
|
||||
while(!hasReceivedFirstOdom() && ros::ok())
|
||||
{
|
||||
ROS_DEBUG_STREAM_THROTTLE(0.5, "Waiting for odom messages to be published.");
|
||||
ros::Duration(0.01).sleep();
|
||||
}
|
||||
if (!ros::ok())
|
||||
FAIL() << "Something went wrong while executing test.";
|
||||
}
|
||||
|
||||
private:
|
||||
bool received_first_odom;
|
||||
ros::NodeHandle nh;
|
||||
ros::Publisher cmd_pub;
|
||||
ros::Subscriber odom_sub;
|
||||
ros::Subscriber vel_out_sub;
|
||||
nav_msgs::Odometry last_odom;
|
||||
geometry_msgs::TwistStamped last_cmd_vel_out;
|
||||
ros::Subscriber joint_traj_controller_state_sub;
|
||||
control_msgs::JointTrajectoryControllerState last_joint_traj_controller_state;
|
||||
|
||||
ros::ServiceClient start_srv;
|
||||
ros::ServiceClient stop_srv;
|
||||
|
||||
void odomCallback(const nav_msgs::Odometry& odom)
|
||||
{
|
||||
ROS_INFO_STREAM("Callback received: 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;
|
||||
received_first_odom = true;
|
||||
}
|
||||
|
||||
void jointTrajectoryControllerStateCallback(const control_msgs::JointTrajectoryControllerState& joint_traj_controller_state)
|
||||
{
|
||||
ROS_INFO_STREAM("Joint trajectory controller state callback.");
|
||||
ROS_DEBUG_STREAM("Joint trajectory controller state callback received:\n" <<
|
||||
joint_traj_controller_state);
|
||||
|
||||
last_joint_traj_controller_state = joint_traj_controller_state;
|
||||
}
|
||||
|
||||
void cmdVelOutCallback(const geometry_msgs::TwistStamped& cmd_vel_out)
|
||||
{
|
||||
ROS_INFO_STREAM("Callback received: lin: " << cmd_vel_out.twist.linear.x
|
||||
<< ", ang: " << cmd_vel_out.twist.angular.z);
|
||||
last_cmd_vel_out = cmd_vel_out;
|
||||
}
|
||||
};
|
||||
|
||||
inline tf::Quaternion tfQuatFromGeomQuat(const geometry_msgs::Quaternion& quat)
|
||||
{
|
||||
return tf::Quaternion(quat.x, quat.y, quat.z, quat.w);
|
||||
}
|
||||
@@ -0,0 +1,26 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- start world -->
|
||||
<include file="$(find gazebo_worlds)/launch/empty_world.launch"/>
|
||||
|
||||
<!-- load robot -->
|
||||
<param name="robot_description"
|
||||
command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot.xacro'" />
|
||||
|
||||
<!-- Start diffbot -->
|
||||
<node name="diffbot"
|
||||
pkg="diff_drive_controller"
|
||||
type="diffbot"/>
|
||||
|
||||
<!-- Load controller config -->
|
||||
<rosparam command="load" file="$(find diff_drive_controller)/test/diffbot_controllers.yaml" />
|
||||
|
||||
<!-- Spawn controller -->
|
||||
<node name="controller_spawner"
|
||||
pkg="controller_manager" type="spawner" output="screen"
|
||||
args="diffbot_controller" />
|
||||
|
||||
<!-- push robot_description to factory and spawn robot in gazebo -->
|
||||
<node name="spawn_single_link" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -model diffbot -z 0.5" respawn="false" output="screen" />
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,26 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- start world -->
|
||||
<include file="$(find gazebo_worlds)/launch/empty_world.launch"/>
|
||||
|
||||
<!-- load robot -->
|
||||
<param name="robot_description"
|
||||
command="$(find xacro)/xacro '$(find diff_drive_controller)/test/skidsteerbot.xacro'" />
|
||||
|
||||
<!-- Start skidsteerbot -->
|
||||
<node name="skidsteerbot"
|
||||
pkg="diff_drive_controller"
|
||||
type="skidsteerbot"/>
|
||||
|
||||
<!-- Load controller config -->
|
||||
<rosparam command="load" file="$(find diff_drive_controller)/test/skidsteerbot_controllers.yaml" />
|
||||
|
||||
<!-- Spawn controller -->
|
||||
<node name="controller_spawner"
|
||||
pkg="controller_manager" type="spawner" output="screen"
|
||||
args="skidsteerbot_controller" />
|
||||
|
||||
<!-- push robot_description to factory and spawn robot in gazebo -->
|
||||
<node name="spawn_single_link" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -model skidsteerbot -z 0.5" respawn="false" output="screen" />
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,43 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<xacro:macro name="wheel" params="name parent radius thickness *origin">
|
||||
<link name="${name}_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}_joint" type="continuous">
|
||||
<parent link="${parent}_link"/>
|
||||
<child link="${name}_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}_joint_trans" type="SimpleTransmission">
|
||||
<actuator name="${name}_joint_motor"/>
|
||||
<joint name="${name}_joint"/>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<motorTorqueConstant>1</motorTorqueConstant>
|
||||
</transmission>
|
||||
|
||||
<gazebo reference="${name}_link">
|
||||
<material>Gazebo/Red</material>
|
||||
</gazebo>
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
@@ -0,0 +1,146 @@
|
||||
cmake_minimum_required(VERSION 3.0.2)
|
||||
project(ros_lift_controller)
|
||||
|
||||
## Compile as C++11, supported in ROS Kinetic and newer
|
||||
# add_compile_options(-std=c++11)
|
||||
|
||||
## Find catkin macros and libraries
|
||||
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
|
||||
## is used, also find other catkin packages
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
gazebo_msgs
|
||||
roscpp
|
||||
rospy
|
||||
gazebo_plugins
|
||||
message_generation
|
||||
std_msgs
|
||||
)
|
||||
|
||||
## System dependencies are found with CMake's conventions
|
||||
find_package(gazebo REQUIRED)
|
||||
find_package(Boost REQUIRED COMPONENTS thread)
|
||||
find_package(Eigen3)
|
||||
if(NOT EIGEN3_FOUND)
|
||||
# Fallback to cmake_modules
|
||||
find_package(cmake_modules REQUIRED)
|
||||
find_package(Eigen REQUIRED)
|
||||
set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
|
||||
else()
|
||||
set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
|
||||
endif()
|
||||
|
||||
## Uncomment this if the package has a setup.py. This macro ensures
|
||||
## modules and global scripts declared therein get installed
|
||||
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
|
||||
# catkin_python_setup()
|
||||
|
||||
################################################
|
||||
## Declare ROS messages, services and actions ##
|
||||
################################################
|
||||
## Generate messages in the 'msg' folder
|
||||
add_message_files(
|
||||
FILES
|
||||
Lift_Measurement.msg
|
||||
)
|
||||
|
||||
## Generate added messages and services with any dependencies listed here
|
||||
generate_messages(
|
||||
DEPENDENCIES
|
||||
std_msgs
|
||||
)
|
||||
|
||||
###################################
|
||||
## catkin specific configuration ##
|
||||
###################################
|
||||
## The catkin_package macro generates cmake config files for your package
|
||||
## Declare things to be passed to dependent projects
|
||||
## INCLUDE_DIRS: uncomment this if your package contains header files
|
||||
## LIBRARIES: libraries you create in this project that dependent projects also need
|
||||
## CATKIN_DEPENDS: catkin_packages dependent projects also need
|
||||
## DEPENDS: system dependencies of this project that dependent projects also need
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
|
||||
DEPENDS Boost
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
## Specify additional locations of header files
|
||||
## Your package locations should be listed before other locations
|
||||
include_directories(
|
||||
include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${Boost_INCLUDE_DIRS}
|
||||
${GAZEBO_INCLUDE_DIRS}
|
||||
${EIGEN3_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
## Declare a C++ library
|
||||
add_library(${PROJECT_NAME}
|
||||
src/ros_jack_lifter.cpp
|
||||
)
|
||||
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
## Specify libraries to link a library or executable target against
|
||||
target_link_libraries(${PROJECT_NAME}
|
||||
${catkin_LIBRARIES}
|
||||
${Boost_LIBRARIES}
|
||||
${GAZEBO_LIBRARIES}
|
||||
)
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
# all install targets should use catkin DESTINATION variables
|
||||
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
|
||||
|
||||
## Mark executable scripts (Python etc.) for installation
|
||||
## in contrast to setup.py, you can choose the destination
|
||||
# catkin_install_python(PROGRAMS
|
||||
# scripts/my_python_script
|
||||
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark executables for installation
|
||||
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
|
||||
# install(TARGETS ${PROJECT_NAME}_node
|
||||
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark libraries for installation
|
||||
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
|
||||
install(TARGETS ${PROJECT_NAME}
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
|
||||
)
|
||||
|
||||
## Mark cpp header files for installation
|
||||
install(DIRECTORY include/${PROJECT_NAME}/
|
||||
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
FILES_MATCHING PATTERN "*.h"
|
||||
PATTERN ".svn" EXCLUDE
|
||||
)
|
||||
|
||||
## Mark other files for installation (e.g. launch and bag files, etc.)
|
||||
install(FILES
|
||||
ros_lift_controller_plugins.xml
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
)
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
## Add gtest based cpp test target and link libraries
|
||||
# catkin_add_gtest(${PROJECT_NAME}-test test/test_ros_lift_controller.cpp)
|
||||
# if(TARGET ${PROJECT_NAME}-test)
|
||||
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
|
||||
# endif()
|
||||
|
||||
## Add folders to be run by python nosetests
|
||||
# catkin_add_nosetests(test)
|
||||
@@ -0,0 +1,104 @@
|
||||
#ifndef GAZEBO_JACKING_LIFTER_PLUGIN_H_INCLUDED_
|
||||
#define GAZEBO_JACKING_LIFTER_PLUGIN_H_INCLUDED_
|
||||
|
||||
#include <map>
|
||||
// Gazebo
|
||||
#include <gazebo_plugins/gazebo_ros_utils.h>
|
||||
|
||||
// ROS
|
||||
#include <ros/ros.h>
|
||||
#include <tf/transform_broadcaster.h>
|
||||
#include <tf/transform_listener.h>
|
||||
#include <geometry_msgs/Twist.h>
|
||||
#include <geometry_msgs/Pose2D.h>
|
||||
#include <geometry_msgs/Pose.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <sensor_msgs/JointState.h>
|
||||
|
||||
// Custom Callback Queue
|
||||
#include <ros/callback_queue.h>
|
||||
#include <ros/advertise_options.h>
|
||||
|
||||
// Boost
|
||||
#include <boost/thread.hpp>
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
namespace gazebo
|
||||
{
|
||||
// class Joint;
|
||||
// class Entity;
|
||||
|
||||
|
||||
class RosLiftLifter : public ModelPlugin
|
||||
{
|
||||
public:
|
||||
RosLiftLifter();
|
||||
~RosLiftLifter();
|
||||
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
|
||||
|
||||
protected:
|
||||
virtual void UpdateChild();
|
||||
virtual void FiniChild();
|
||||
|
||||
private:
|
||||
GazeboRosPtr gazebo_ros_;
|
||||
physics::ModelPtr parent;
|
||||
//void publishOdometry(double step_time);
|
||||
void publishLiftSensorStates();
|
||||
void publishTF(physics::JointPtr joint); /// publishes the wheel tf's
|
||||
void publishJointState(physics::JointPtr joint);
|
||||
void effortController(physics::JointPtr joint, const double &error, const double &dt);
|
||||
void positionController(physics::JointPtr joint, const double &target, double ¤t , const double &dt);
|
||||
|
||||
event::ConnectionPtr update_connection_;
|
||||
physics::JointPtr base_lifting_joint_, base_rotating_joint_;
|
||||
double lift_torque_, rotate_torque_;
|
||||
|
||||
std::string robot_namespace_;
|
||||
std::string command_topic_;
|
||||
std::string robot_base_frame_;
|
||||
|
||||
// ROS STUFF
|
||||
ros::Subscriber cmd_lift_subscriber_;
|
||||
boost::shared_ptr<tf::TransformBroadcaster> transform_broadcaster_;
|
||||
ros::Publisher joint_state_publisher_;
|
||||
ros::Publisher state_lift_publisher_;
|
||||
|
||||
boost::mutex lock;
|
||||
|
||||
// Custom Callback Queue
|
||||
ros::CallbackQueue queue_;
|
||||
boost::thread callback_queue_thread_;
|
||||
void QueueThread();
|
||||
|
||||
// DiffDrive stuff
|
||||
void cmdCallback(const geometry_msgs::Pose::ConstPtr& cmd_msg);
|
||||
/// updates the relative robot pose based on the wheel encoders
|
||||
void UpdateForkEncoder();
|
||||
|
||||
double lift_height_cmd_, current_lift_position_, lift_step_;
|
||||
double lift_rotate_cmd_, current_lift_angle_, rotate_step_;
|
||||
double lift_height_maximum_;
|
||||
|
||||
bool alive_;
|
||||
double lift_height_encoder_;
|
||||
common::Time last_encoder_update_;
|
||||
double lift_height_origin_;
|
||||
common::PID joint_lift_pid_, joint_rotate_pid_;
|
||||
|
||||
// Update Rate
|
||||
double update_rate_;
|
||||
double update_period_;
|
||||
common::Time last_actuator_update_;
|
||||
|
||||
// Flags
|
||||
bool publishTF_;
|
||||
bool publishJointState_;
|
||||
|
||||
// bool use_velocity_control_;
|
||||
// double max_velocity_;
|
||||
};
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,5 @@
|
||||
Header header
|
||||
|
||||
float32 angle_position # Angle to the X axis's of base_link and X axis's of surface [rad]
|
||||
bool top_sensor
|
||||
bool bottom_sensor
|
||||
@@ -0,0 +1,90 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>ros_lift_controller</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The ros_lift_controller package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="robotics@todo.todo">robotics</maintainer>
|
||||
|
||||
|
||||
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||
<!-- Commonly used license strings: -->
|
||||
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
||||
<license>TODO</license>
|
||||
|
||||
|
||||
<!-- Url tags are optional, but multiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
<!-- Example: -->
|
||||
<!-- <url type="website">http://wiki.ros.org/ros_lift_controller</url> -->
|
||||
|
||||
|
||||
<!-- Author tags are optional, multiple are allowed, one per tag -->
|
||||
<!-- Authors do not have to be maintainers, but could be -->
|
||||
<!-- Example: -->
|
||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||
|
||||
|
||||
<!-- The *depend tags are used to specify dependencies -->
|
||||
<!-- Dependencies can be catkin packages or system dependencies -->
|
||||
<!-- Examples: -->
|
||||
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
|
||||
<!-- <depend>roscpp</depend> -->
|
||||
<!-- Note that this is equivalent to the following: -->
|
||||
<!-- <build_depend>roscpp</build_depend> -->
|
||||
<!-- <exec_depend>roscpp</exec_depend> -->
|
||||
<!-- Use build_depend for packages you need at compile time: -->
|
||||
<build_depend>message_generation</build_depend>
|
||||
<!-- Use build_export_depend for packages you need in order to build against this package: -->
|
||||
<!-- <build_export_depend>message_generation</build_export_depend> -->
|
||||
<!-- Use buildtool_depend for build tool packages: -->
|
||||
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
||||
<!-- Use exec_depend for packages you need at runtime: -->
|
||||
<exec_depend>message_runtime</exec_depend>
|
||||
<!-- Use test_depend for packages you need only for testing: -->
|
||||
<!-- <test_depend>gtest</test_depend> -->
|
||||
<!-- Use doc_depend for packages you need only for building documentation: -->
|
||||
<!-- <doc_depend>doxygen</doc_depend> -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>gazebo8</build_depend>
|
||||
<build_depend>gazebo8_msgs</build_depend>
|
||||
<build_depend>gazebo8_plugins</build_depend>
|
||||
<build_depend>gazebo8_ros</build_depend>
|
||||
<build_depend>orunav_generic</build_depend>
|
||||
<build_depend>eigen</build_depend>
|
||||
<build_depend>boost</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>rospy</build_export_depend>
|
||||
<build_export_depend>gazebo8</build_export_depend>
|
||||
<build_export_depend>gazebo8_msgs</build_export_depend>
|
||||
<build_export_depend>gazebo8_plugins</build_export_depend>
|
||||
<build_export_depend>gazebo8_ros</build_export_depend>
|
||||
<build_export_depend>orunav_generic</build_export_depend>
|
||||
<build_export_depend>eigen</build_export_depend>
|
||||
<build_export_depend>boost</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>rospy</exec_depend>
|
||||
<exec_depend>gazebo8</exec_depend>
|
||||
<exec_depend>gazebo8_msgs</exec_depend>
|
||||
<exec_depend>gazebo8_plugins</exec_depend>
|
||||
<exec_depend>gazebo8_ros</exec_depend>
|
||||
<exec_depend>orunav_generic</exec_depend>
|
||||
<exec_depend>eigen</exec_depend>
|
||||
<exec_depend>boost</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<gazebo plugin_path="${prefix}/lib" />
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,335 @@
|
||||
/**
|
||||
* \file gazebo_ros_steer_drive.cpp
|
||||
* \brief A fork lifter plugin for gazebo - taken from the old SAUNA repo.
|
||||
* \author Henrik Andreasson <henrik.andreasson@oru.se>
|
||||
*/
|
||||
|
||||
|
||||
#include <algorithm>
|
||||
#include <assert.h>
|
||||
|
||||
#include <ros_lift_controller/ros_jack_lifter.h>
|
||||
|
||||
//#include <gazebo/math/gzmath.hh>
|
||||
#include <sdf/sdf.hh>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <tf/transform_broadcaster.h>
|
||||
#include <tf/transform_listener.h>
|
||||
#include <geometry_msgs/Twist.h>
|
||||
#include <nav_msgs/GetMap.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <boost/bind.hpp>
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <ros_lift_controller/Lift_Measurement.h>
|
||||
|
||||
namespace gazebo
|
||||
{
|
||||
RosLiftLifter::RosLiftLifter() {}
|
||||
|
||||
// Destructor
|
||||
RosLiftLifter::~RosLiftLifter() {}
|
||||
|
||||
// Load the controller
|
||||
void RosLiftLifter::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
|
||||
{
|
||||
parent = _parent;
|
||||
gazebo_ros_ = GazeboRosPtr(new GazeboRos(_parent, _sdf, "ForkLifter"));
|
||||
// Make sure the ROS node for Gazebo has already been initialized
|
||||
gazebo_ros_->isInitialized();
|
||||
|
||||
gazebo_ros_->getParameter<double>(update_rate_, "updateRate", 100.0);
|
||||
|
||||
gazebo_ros_->getParameter<double>(lift_torque_, "liftTorque", 1000.0);
|
||||
gazebo_ros_->getParameter<double>(rotate_torque_, "rotateTorque", 1000.0);
|
||||
gazebo_ros_->getParameter<double>(lift_height_maximum_, "liftHeightMaximum", 0.1);
|
||||
|
||||
gazebo_ros_->getParameter<std::string>(robot_base_frame_, "robotBaseFrame", "base_link");
|
||||
|
||||
// Gazebo specific height to align the forks to the origin.
|
||||
gazebo_ros_->getParameter<double>(lift_height_origin_, "liftHeightOrigin", 0.08);
|
||||
|
||||
gazebo_ros_->getParameterBoolean(publishTF_, "publishTF", false);
|
||||
gazebo_ros_->getParameterBoolean(publishJointState_, "publishJointState", false);
|
||||
|
||||
gazebo_ros_->getParameter<std::string>(command_topic_, "commandTopic", "cmd_jacking");
|
||||
// gazebo_ros_->getParameterBoolean(use_velocity_control_, "useVelocityControl", true);
|
||||
// gazebo_ros_->getParameter<double>(max_velocity_, "maxVelocity", 0.08);
|
||||
|
||||
gazebo_ros_->getParameter<double>(lift_step_, "liftStep", 0.002);
|
||||
gazebo_ros_->getParameter<double>(rotate_step_, "rotateStep", 0.05);
|
||||
|
||||
double pid_p, pid_i, pid_d;
|
||||
gazebo_ros_->getParameter<double>(pid_p, "pidP", 2000);
|
||||
gazebo_ros_->getParameter<double>(pid_i, "pidI", 0);
|
||||
gazebo_ros_->getParameter<double>(pid_d, "pidD", 10);
|
||||
|
||||
base_lifting_joint_ = gazebo_ros_->getJoint(parent, "liftJoint", "base_lifting_joint");
|
||||
base_rotating_joint_ = gazebo_ros_->getJoint(parent, "rotateJoint", "base_rotating_joint");
|
||||
#if GAZEBO_MAJOR_VERSION > 2
|
||||
base_lifting_joint_->SetParam("fmax", 0, lift_torque_);
|
||||
base_rotating_joint_->SetParam("fmax", 0, rotate_torque_);
|
||||
#else
|
||||
base_lifting_joint_->SetMaxForce(0, lift_torque_);
|
||||
base_rotating_joint_->SetMaxForce(0, rotate_torque_);
|
||||
#endif
|
||||
|
||||
// Initialize update rate stuff
|
||||
if (this->update_rate_ > 0.0) this->update_period_ = 1.0 / this->update_rate_;
|
||||
else this->update_period_ = 0.0;
|
||||
|
||||
#if GAZEBO_MAJOR_VERSION >= 8
|
||||
last_actuator_update_ = parent->GetWorld()->SimTime();
|
||||
#else
|
||||
last_actuator_update_ = parent->GetWorld()->GetSimTime();
|
||||
#endif
|
||||
|
||||
// Initialize velocity stuff
|
||||
alive_ = true;
|
||||
this->joint_lift_pid_.Init(pid_p, pid_i, pid_d, 100, -100, lift_torque_, -lift_torque_);
|
||||
this->joint_rotate_pid_.Init(pid_p, pid_i, pid_d, 100, -100, rotate_torque_, -rotate_torque_);
|
||||
|
||||
if (this->publishJointState_) {
|
||||
joint_state_publisher_ = gazebo_ros_->node()->advertise<sensor_msgs::JointState>("joint_states", 1000);
|
||||
ROS_INFO("%s: Advertise joint_states!", gazebo_ros_->info());
|
||||
}
|
||||
|
||||
transform_broadcaster_ = boost::shared_ptr<tf::TransformBroadcaster>(new tf::TransformBroadcaster());
|
||||
|
||||
// ROS: Subscribe to the velocity command topic (cmd_fork)
|
||||
ROS_INFO("%s: Try to subscribe to %s!", gazebo_ros_->info(), command_topic_.c_str());
|
||||
|
||||
ros::SubscribeOptions so =
|
||||
ros::SubscribeOptions::create<geometry_msgs::Pose>(command_topic_, 1,
|
||||
boost::bind(&RosLiftLifter::cmdCallback, this, _1),
|
||||
ros::VoidPtr(), &queue_);
|
||||
|
||||
cmd_lift_subscriber_ = gazebo_ros_->node()->subscribe(so);
|
||||
ROS_INFO("%s: Subscribe to %s!", gazebo_ros_->info(), command_topic_.c_str());
|
||||
state_lift_publisher_ = gazebo_ros_->node()->advertise<ros_lift_controller::Lift_Measurement>("lift_sensor_states", 100);
|
||||
|
||||
// start custom queue for diff drive
|
||||
this->callback_queue_thread_ = boost::thread(boost::bind(&RosLiftLifter::QueueThread, this));
|
||||
|
||||
// listen to the update event (broadcast every simulation iteration)
|
||||
this->update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&RosLiftLifter::UpdateChild, this));
|
||||
|
||||
lift_height_cmd_ = 0.0;
|
||||
current_lift_position_ = 0.0;
|
||||
lift_rotate_cmd_= 0.0;
|
||||
current_lift_angle_ = 0.0;
|
||||
}
|
||||
|
||||
void RosLiftLifter::publishLiftSensorStates()
|
||||
{
|
||||
ros_lift_controller::Lift_Measurement lm;
|
||||
lm.header.stamp = ros::Time::now();
|
||||
lm.header.frame_id = robot_base_frame_;
|
||||
|
||||
#if GAZEBO_MAJOR_VERSION >= 8
|
||||
ignition::math::Pose3d pose_rotate = base_rotating_joint_->GetChild()->RelativePose();
|
||||
#else
|
||||
ignition::math::Pose3d pose_rotate = base_rotating_joint_->GetChild()->GetRelativePose().Ign();
|
||||
#endif
|
||||
|
||||
lm.angle_position = pose_rotate.Rot().Euler().Z();
|
||||
|
||||
lm.top_sensor = fabs(current_lift_position_ - lift_height_maximum_) < 0.01? true : false;
|
||||
lm.bottom_sensor = fabs(current_lift_position_ - 0.0) < 0.01? true : false;
|
||||
state_lift_publisher_.publish(lm);
|
||||
}
|
||||
|
||||
void RosLiftLifter::publishJointState(physics::JointPtr joint)
|
||||
{
|
||||
std::vector<physics::JointPtr> joints;
|
||||
joints.push_back(joint);
|
||||
|
||||
ros::Time current_time = ros::Time::now();
|
||||
sensor_msgs::JointState joint_state;
|
||||
joint_state.header.stamp = current_time;
|
||||
joint_state.name.resize(joints.size());
|
||||
joint_state.position.resize(joints.size());
|
||||
joint_state.velocity.resize(joints.size());
|
||||
joint_state.effort.resize(joints.size());
|
||||
for (std::size_t i = 0; i < joints.size(); i++) {
|
||||
joint_state.name[i] = joints[i]->GetName();
|
||||
#if GAZEBO_MAJOR_VERSION >= 8
|
||||
joint_state.position[i] = joints[i]->Position(0);
|
||||
#else
|
||||
joint_state.position[i] = joints[i]->GetAngle(0).Radian();
|
||||
#endif
|
||||
joint_state.velocity[i] = joints[i]->GetVelocity(0);
|
||||
joint_state.effort[i] = joints[i]->GetForce(0);
|
||||
}
|
||||
joint_state_publisher_.publish(joint_state);
|
||||
}
|
||||
|
||||
void RosLiftLifter::publishTF(physics::JointPtr joint)
|
||||
{
|
||||
ros::Time current_time = ros::Time::now();
|
||||
std::vector<physics::JointPtr> joints;
|
||||
joints.push_back(joint);
|
||||
|
||||
for (std::size_t i = 0; i < joints.size(); i++) {
|
||||
std::string frame = gazebo_ros_->resolveTF(joints[i]->GetName());
|
||||
std::string parent_frame = gazebo_ros_->resolveTF(joints[i]->GetParent()->GetName());
|
||||
|
||||
#if GAZEBO_MAJOR_VERSION >= 8
|
||||
ignition::math::Pose3d pose = joints[i]->GetChild()->RelativePose();
|
||||
#else
|
||||
ignition::math::Pose3d pose = joints[i]->GetChild()->GetRelativePose().Ign();
|
||||
#endif
|
||||
|
||||
tf::Quaternion qt(pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z(), pose.Rot().W());
|
||||
tf::Vector3 vt(pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z());
|
||||
|
||||
tf::Transform transform(qt, vt);
|
||||
transform_broadcaster_->sendTransform(tf::StampedTransform(transform, current_time, parent_frame, frame));
|
||||
}
|
||||
|
||||
}
|
||||
// Update the controller
|
||||
void RosLiftLifter::UpdateChild()
|
||||
{
|
||||
UpdateForkEncoder();
|
||||
#if GAZEBO_MAJOR_VERSION >= 8
|
||||
common::Time current_time = parent->GetWorld()->SimTime();
|
||||
#else
|
||||
common::Time current_time = parent->GetWorld()->GetSimTime();
|
||||
#endif
|
||||
|
||||
double seconds_since_last_update = (current_time - last_actuator_update_).Double();
|
||||
if (seconds_since_last_update > update_period_) {
|
||||
|
||||
if (publishJointState_)
|
||||
{
|
||||
publishJointState(base_lifting_joint_);
|
||||
publishJointState(base_rotating_joint_);
|
||||
}
|
||||
publishLiftSensorStates();
|
||||
|
||||
// Use the PID class...
|
||||
#if GAZEBO_MAJOR_VERSION >= 8
|
||||
ignition::math::Pose3d pose_lift = base_lifting_joint_->GetChild()->RelativePose();
|
||||
#else
|
||||
ignition::math::Pose3d pose_lift = base_lifting_joint_->GetChild()->GetRelativePose().Ign();
|
||||
#endif
|
||||
double target_lift_height = lift_height_cmd_ + this->lift_height_origin_;
|
||||
double current_lift_height = pose_lift.Pos().Z();
|
||||
double error_lift_height = current_lift_height - target_lift_height;
|
||||
this->effortController(base_lifting_joint_, error_lift_height, seconds_since_last_update);
|
||||
this->positionController(base_lifting_joint_, lift_height_cmd_, current_lift_position_, lift_step_);
|
||||
|
||||
#if GAZEBO_MAJOR_VERSION >= 8
|
||||
ignition::math::Pose3d pose_rotate = base_rotating_joint_->GetChild()->RelativePose();
|
||||
#else
|
||||
ignition::math::Pose3d pose_rotate = base_rotating_joint_->GetChild()->GetRelativePose().Ign();
|
||||
#endif
|
||||
|
||||
double target_lift_yaw = lift_rotate_cmd_;
|
||||
double current_lift_yaw = pose_rotate.Rot().Euler().Z();
|
||||
double error_lift_yaw = current_lift_yaw - target_lift_yaw;
|
||||
this->effortController(base_rotating_joint_, error_lift_yaw, seconds_since_last_update);
|
||||
this->positionController(base_rotating_joint_, lift_rotate_cmd_, current_lift_angle_, rotate_step_);
|
||||
|
||||
last_actuator_update_ += common::Time(update_period_);
|
||||
}
|
||||
|
||||
#if 0
|
||||
if (odom_source_ == ENCODER) UpdateOdometryEncoder();
|
||||
common::Time current_time = parent->GetWorld()->GetSimTime();
|
||||
double seconds_since_last_update = (current_time - last_actuator_update_).Double();
|
||||
if (seconds_since_last_update > update_period_) {
|
||||
|
||||
publishOdometry(seconds_since_last_update);
|
||||
if (publishWheelTF_) publishWheelTF();
|
||||
if (publishWheelJointState_) publishWheelJointState();
|
||||
|
||||
double target_wheel_roation_speed = cmd_.speed / (wheel_diameter_ / 2.0);
|
||||
double target_steering_angle_speed = cmd_.angle;
|
||||
|
||||
motorController(target_wheel_roation_speed, target_steering_angle_speed, seconds_since_last_update);
|
||||
|
||||
//ROS_INFO("v = %f, w = %f !", target_wheel_roation_speed, target_steering_angle);
|
||||
|
||||
last_actuator_update_ += common::Time(update_period_);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void RosLiftLifter::effortController(physics::JointPtr joint, const double &error, const double &dt)
|
||||
{
|
||||
double control_value = this->joint_lift_pid_.Update(error, dt);
|
||||
#if GAZEBO_MAJOR_VERSION > 2
|
||||
joint->SetForce(0, control_value);
|
||||
#else
|
||||
joint->SetForce(0, control_value);
|
||||
#endif
|
||||
}
|
||||
|
||||
void RosLiftLifter::positionController(physics::JointPtr joint, const double &target, double ¤t , const double &dt)
|
||||
{
|
||||
double error = target - current;
|
||||
double step = dt;
|
||||
// Nếu sai số lớn, tăng/giảm dần
|
||||
if (std::abs(error) > step)
|
||||
{
|
||||
current += step * (error > 0 ? 1 : -1);
|
||||
}
|
||||
else
|
||||
{
|
||||
current = target; // gần bằng rồi, gán luôn
|
||||
}
|
||||
|
||||
#if GAZEBO_MAJOR_VERSION > 2
|
||||
joint->SetPosition(0, current);
|
||||
#else
|
||||
joint->SetPosition(0, current);
|
||||
#endif
|
||||
}
|
||||
|
||||
// Finalize the controller
|
||||
void RosLiftLifter::FiniChild()
|
||||
{
|
||||
alive_ = false;
|
||||
queue_.clear();
|
||||
queue_.disable();
|
||||
gazebo_ros_->node()->shutdown();
|
||||
callback_queue_thread_.join();
|
||||
}
|
||||
|
||||
void RosLiftLifter::cmdCallback(const geometry_msgs::Pose::ConstPtr& cmd_msg)
|
||||
{
|
||||
boost::mutex::scoped_lock scoped_lock(lock);
|
||||
lift_height_cmd_ = std::min(lift_height_maximum_, cmd_msg->position.z);
|
||||
lift_rotate_cmd_ = tf::getYaw(cmd_msg->orientation);
|
||||
}
|
||||
|
||||
void RosLiftLifter::QueueThread()
|
||||
{
|
||||
static const double timeout = 0.01;
|
||||
|
||||
while (alive_ && gazebo_ros_->node()->ok()) {
|
||||
queue_.callAvailable(ros::WallDuration(timeout));
|
||||
}
|
||||
}
|
||||
|
||||
void RosLiftLifter::UpdateForkEncoder()
|
||||
{
|
||||
#if GAZEBO_MAJOR_VERSION >= 8
|
||||
common::Time current_time = parent->GetWorld()->SimTime();
|
||||
#else
|
||||
common::Time current_time = parent->GetWorld()->GetSimTime();
|
||||
#endif
|
||||
double step_time = (current_time - last_encoder_update_).Double();
|
||||
last_encoder_update_ = current_time;
|
||||
#if GAZEBO_MAJOR_VERSION >= 8
|
||||
ignition::math::Pose3d pose = base_lifting_joint_->GetChild()->RelativePose();
|
||||
#else
|
||||
ignition::math::Pose3d pose = base_lifting_joint_->GetChild()->GetRelativePose().Ign();
|
||||
#endif
|
||||
lift_height_encoder_ = pose.Pos().Z();
|
||||
}
|
||||
GZ_REGISTER_MODEL_PLUGIN(RosLiftLifter)
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user