From 632e506a468219923e6284014d6a2ba0af174b0d Mon Sep 17 00:00:00 2001 From: Antoine Rennuit Date: Mon, 27 Oct 2014 17:21:32 +0100 Subject: [PATCH 01/21] Made an exact copy of diff_drive_controller and called it mecanum_drive_controller. --- mecanum_drive_controller/CHANGELOG.rst | 35 ++ mecanum_drive_controller/CMakeLists.txt | 69 +++ mecanum_drive_controller/README.md | 5 + .../diff_drive_controller_plugins.xml | 9 + .../diff_drive_controller.h | 200 +++++++ .../include/diff_drive_controller/odometry.h | 199 +++++++ .../diff_drive_controller/speed_limiter.h | 105 ++++ mecanum_drive_controller/package.xml | 39 ++ .../src/diff_drive_controller.cpp | 514 ++++++++++++++++++ mecanum_drive_controller/src/odometry.cpp | 164 ++++++ .../src/speed_limiter.cpp | 98 ++++ .../test/diff_drive_bad_urdf.test | 17 + .../test/diff_drive_common.launch | 31 ++ .../test/diff_drive_controller.test | 13 + .../test/diff_drive_controller_limits.test | 16 + .../test/diff_drive_fail_test.cpp | 57 ++ .../test/diff_drive_limits_test.cpp | 169 ++++++ .../test/diff_drive_multipliers.test | 16 + .../test/diff_drive_odom_tf.test | 19 + .../test/diff_drive_odom_tf_test.cpp | 59 ++ .../test/diff_drive_open_loop.test | 16 + .../test/diff_drive_test.cpp | 151 +++++ .../test/diff_drive_timeout.test | 16 + .../test/diff_drive_timeout_test.cpp | 72 +++ .../test/diff_drive_wrong.test | 16 + mecanum_drive_controller/test/diffbot.cpp | 60 ++ mecanum_drive_controller/test/diffbot.h | 139 +++++ mecanum_drive_controller/test/diffbot.xacro | 76 +++ .../test/diffbot_bad.xacro | 142 +++++ .../test/diffbot_controllers.yaml | 8 + .../test/diffbot_limits.yaml | 15 + .../test/diffbot_multipliers.yaml | 3 + .../test/diffbot_open_loop.yaml | 2 + .../test/diffbot_timeout.yaml | 2 + .../test/diffbot_wrong.yaml | 8 + .../test/skid_steer_common.launch | 31 ++ .../test/skid_steer_controller.test | 13 + .../test/skid_steer_no_wheels.test | 16 + .../test/skidsteerbot.cpp | 60 ++ .../test/skidsteerbot.xacro | 89 +++ .../test/skidsteerbot_controllers.yaml | 8 + .../test/skidsteerbot_no_wheels.yaml | 8 + mecanum_drive_controller/test/test_common.h | 85 +++ .../test/view_diffbot.launch | 26 + .../test/view_skidsteerbot.launch | 26 + mecanum_drive_controller/test/wheel.xacro | 43 ++ 46 files changed, 2965 insertions(+) create mode 100644 mecanum_drive_controller/CHANGELOG.rst create mode 100644 mecanum_drive_controller/CMakeLists.txt create mode 100644 mecanum_drive_controller/README.md create mode 100644 mecanum_drive_controller/diff_drive_controller_plugins.xml create mode 100644 mecanum_drive_controller/include/diff_drive_controller/diff_drive_controller.h create mode 100644 mecanum_drive_controller/include/diff_drive_controller/odometry.h create mode 100644 mecanum_drive_controller/include/diff_drive_controller/speed_limiter.h create mode 100644 mecanum_drive_controller/package.xml create mode 100644 mecanum_drive_controller/src/diff_drive_controller.cpp create mode 100644 mecanum_drive_controller/src/odometry.cpp create mode 100644 mecanum_drive_controller/src/speed_limiter.cpp create mode 100644 mecanum_drive_controller/test/diff_drive_bad_urdf.test create mode 100644 mecanum_drive_controller/test/diff_drive_common.launch create mode 100644 mecanum_drive_controller/test/diff_drive_controller.test create mode 100644 mecanum_drive_controller/test/diff_drive_controller_limits.test create mode 100644 mecanum_drive_controller/test/diff_drive_fail_test.cpp create mode 100644 mecanum_drive_controller/test/diff_drive_limits_test.cpp create mode 100644 mecanum_drive_controller/test/diff_drive_multipliers.test create mode 100644 mecanum_drive_controller/test/diff_drive_odom_tf.test create mode 100644 mecanum_drive_controller/test/diff_drive_odom_tf_test.cpp create mode 100644 mecanum_drive_controller/test/diff_drive_open_loop.test create mode 100644 mecanum_drive_controller/test/diff_drive_test.cpp create mode 100644 mecanum_drive_controller/test/diff_drive_timeout.test create mode 100644 mecanum_drive_controller/test/diff_drive_timeout_test.cpp create mode 100644 mecanum_drive_controller/test/diff_drive_wrong.test create mode 100644 mecanum_drive_controller/test/diffbot.cpp create mode 100644 mecanum_drive_controller/test/diffbot.h create mode 100644 mecanum_drive_controller/test/diffbot.xacro create mode 100644 mecanum_drive_controller/test/diffbot_bad.xacro create mode 100644 mecanum_drive_controller/test/diffbot_controllers.yaml create mode 100644 mecanum_drive_controller/test/diffbot_limits.yaml create mode 100644 mecanum_drive_controller/test/diffbot_multipliers.yaml create mode 100644 mecanum_drive_controller/test/diffbot_open_loop.yaml create mode 100644 mecanum_drive_controller/test/diffbot_timeout.yaml create mode 100644 mecanum_drive_controller/test/diffbot_wrong.yaml create mode 100644 mecanum_drive_controller/test/skid_steer_common.launch create mode 100644 mecanum_drive_controller/test/skid_steer_controller.test create mode 100644 mecanum_drive_controller/test/skid_steer_no_wheels.test create mode 100644 mecanum_drive_controller/test/skidsteerbot.cpp create mode 100644 mecanum_drive_controller/test/skidsteerbot.xacro create mode 100644 mecanum_drive_controller/test/skidsteerbot_controllers.yaml create mode 100644 mecanum_drive_controller/test/skidsteerbot_no_wheels.yaml create mode 100644 mecanum_drive_controller/test/test_common.h create mode 100644 mecanum_drive_controller/test/view_diffbot.launch create mode 100644 mecanum_drive_controller/test/view_skidsteerbot.launch create mode 100644 mecanum_drive_controller/test/wheel.xacro diff --git a/mecanum_drive_controller/CHANGELOG.rst b/mecanum_drive_controller/CHANGELOG.rst new file mode 100644 index 000000000..2de77d980 --- /dev/null +++ b/mecanum_drive_controller/CHANGELOG.rst @@ -0,0 +1,35 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package diff_drive_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +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. diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt new file mode 100644 index 000000000..fb63c04c1 --- /dev/null +++ b/mecanum_drive_controller/CMakeLists.txt @@ -0,0 +1,69 @@ +cmake_minimum_required(VERSION 2.8.3) +project(diff_drive_controller) + +find_package(catkin REQUIRED COMPONENTS + controller_interface + urdf + realtime_tools + tf + nav_msgs) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} +) + +include_directories( + include ${catkin_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}) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + +install(FILES diff_drive_controller_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +if (CATKIN_ENABLE_TESTING) + find_package(catkin COMPONENTS rostest std_srvs controller_manager tf) + include_directories(include ${catkin_INCLUDE_DIRS}) + + add_executable(diffbot test/diffbot.cpp) + target_link_libraries(diffbot ${catkin_LIBRARIES}) + + add_executable(skidsteerbot test/skidsteerbot.cpp) + target_link_libraries(skidsteerbot ${catkin_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}) + 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}) + 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}) + add_rostest(test/diff_drive_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}) + add_rostest(test/diff_drive_bad_urdf.test) + 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}) + add_rostest(test/diff_drive_open_loop.test) + add_rostest(test/skid_steer_controller.test) + add_rostest(test/skid_steer_no_wheels.test) +endif() diff --git a/mecanum_drive_controller/README.md b/mecanum_drive_controller/README.md new file mode 100644 index 000000000..371baed1f --- /dev/null +++ b/mecanum_drive_controller/README.md @@ -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). diff --git a/mecanum_drive_controller/diff_drive_controller_plugins.xml b/mecanum_drive_controller/diff_drive_controller_plugins.xml new file mode 100644 index 000000000..3ec80bfa3 --- /dev/null +++ b/mecanum_drive_controller/diff_drive_controller_plugins.xml @@ -0,0 +1,9 @@ + + + + + The DiffDriveController tracks velocity commands. It expects 2 VelocityJointInterface type of hardware interfaces. + + + + diff --git a/mecanum_drive_controller/include/diff_drive_controller/diff_drive_controller.h b/mecanum_drive_controller/include/diff_drive_controller/diff_drive_controller.h new file mode 100644 index 000000000..8392dd087 --- /dev/null +++ b/mecanum_drive_controller/include/diff_drive_controller/diff_drive_controller.h @@ -0,0 +1,200 @@ +/********************************************************************* + * 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 +#include +#include + +#include +#include + +#include +#include + +#include +#include + +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 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 + { + 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 left_wheel_joints_; + std::vector right_wheel_joints_; + + /// 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 command_; + Commands command_struct_; + ros::Subscriber sub_command_; + + /// Odometry related: + boost::shared_ptr > odom_pub_; + boost::shared_ptr > tf_odom_pub_; + Odometry odometry_; + geometry_msgs::TransformStamped odom_frame_; + + /// 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 wheel_radius_multiplier_; + + /// Timeout to consider cmd_vel commands old: + double cmd_vel_timeout_; + + /// Frame to use for the robot base: + std::string base_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 last_cmd_; + SpeedLimiter limiter_lin_; + SpeedLimiter limiter_ang_; + + 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& 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); + + /** + * \brief Sets the odometry publishing fields + * \param root_nh Root node handle + * \param controller_nh Node handle inside the controller namespace + */ + void setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh); + + }; + + PLUGINLIB_EXPORT_CLASS(diff_drive_controller::DiffDriveController, controller_interface::ControllerBase); +} // namespace diff_drive_controller diff --git a/mecanum_drive_controller/include/diff_drive_controller/odometry.h b/mecanum_drive_controller/include/diff_drive_controller/odometry.h new file mode 100644 index 000000000..7c665b537 --- /dev/null +++ b/mecanum_drive_controller/include/diff_drive_controller/odometry.h @@ -0,0 +1,199 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, PAL Robotics, S.L. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PAL Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: Luca Marchionni + * Author: Bence Magyar + * Author: Enrique Fernández + * Author: Paul Mathieu + */ + +#ifndef ODOMETRY_H_ +#define ODOMETRY_H_ + +#include +#include +#include +#include +#include + +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 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 Seperation between left and right wheels [m] + * \param wheel_radius Wheel radius [m] + */ + void setWheelParams(double wheel_separation, double wheel_radius); + + private: + + /// Rolling mean accumulator and window: + typedef bacc::accumulator_set > 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); + + /// 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 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_; + }; +} + +#endif /* ODOMETRY_H_ */ diff --git a/mecanum_drive_controller/include/diff_drive_controller/speed_limiter.h b/mecanum_drive_controller/include/diff_drive_controller/speed_limiter.h new file mode 100644 index 000000000..aac881207 --- /dev/null +++ b/mecanum_drive_controller/include/diff_drive_controller/speed_limiter.h @@ -0,0 +1,105 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, PAL Robotics, S.L. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PAL Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: Enrique Fernández + */ + +#ifndef SPEED_LIMITER_H +#define SPEED_LIMITER_H + +namespace 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] 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 + */ + SpeedLimiter( + bool has_velocity_limits = false, + bool has_acceleration_limits = false, + double min_velocity = 0.0, + double max_velocity = 0.0, + double min_acceleration = 0.0, + double max_acceleration = 0.0 + ); + + /** + * \brief Limit the velocity and acceleration + * \param [in, out] v Velocity [m/s] + * \param [in] v0 Previous velocity [m/s] + * \param [in] dt Time step [s] + */ + void limit(double& v, double v0, double dt); + + /** + * \brief Limit the velocity + * \param [in, out] v Velocity [m/s] + */ + void 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] + */ + void limit_acceleration(double& v, double v0, double dt); + + public: + // Enable/Disable velocity/acceleration limits: + bool has_velocity_limits; + bool has_acceleration_limits; + + // Velocity limits: + double min_velocity; + double max_velocity; + + // Acceleration limits: + double min_acceleration; + double max_acceleration; + }; + +} // namespace diff_drive_controller + +#endif // SPEED_LIMITER_H diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml new file mode 100644 index 000000000..9aa0f0b60 --- /dev/null +++ b/mecanum_drive_controller/package.xml @@ -0,0 +1,39 @@ + + diff_drive_controller + 0.8.1 + Controller for a differential drive mobile base. + Bence Magyar + + BSD + + https://github.com/ros-controls/ros_controllers/wiki + https://github.com/ros-controls/ros_controllers/issues + https://github.com/ros-controls/ros_controllers + + Bence Magyar + + catkin + + controller_interface + nav_msgs + realtime_tools + tf + urdf + + controller_interface + nav_msgs + realtime_tools + tf + urdf + + + rostest + + std_srvs + controller_manager + xacro + + + + + diff --git a/mecanum_drive_controller/src/diff_drive_controller.cpp b/mecanum_drive_controller/src/diff_drive_controller.cpp new file mode 100644 index 000000000..49f6444f7 --- /dev/null +++ b/mecanum_drive_controller/src/diff_drive_controller.cpp @@ -0,0 +1,514 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, PAL Robotics, S.L. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PAL Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: Bence Magyar + */ + +#include + +#include + +#include + +#include + +static double euclideanOfVectors(const urdf::Vector3& vec1, const urdf::Vector3& vec2) +{ + return std::sqrt(std::pow(vec1.x-vec2.x,2) + + std::pow(vec1.y-vec2.y,2) + + std::pow(vec1.z-vec2.z,2)); +} + +/* + * \brief Check if the link is modeled as a cylinder + * \param link Link + * \return true if the link is modeled as a Cylinder; false otherwise + */ +static bool isCylinder(const boost::shared_ptr& link) +{ + if(!link) + { + ROS_ERROR("Link == NULL."); + return false; + } + + if(!link->collision) + { + ROS_ERROR_STREAM("Link " << link->name << " does not have collision description. Add collision description for link to urdf."); + return false; + } + + if(!link->collision->geometry) + { + ROS_ERROR_STREAM("Link " << link->name << " does not have collision geometry description. Add collision geometry description for link to urdf."); + return false; + } + + if(link->collision->geometry->type != urdf::Geometry::CYLINDER) + { + ROS_ERROR_STREAM("Link " << link->name << " does not have cylinder geometry"); + return false; + } + + return true; +} + +/* + * \brief Get the wheel radius + * \param [in] wheel_link Wheel link + * \param [out] wheel_radius Wheel radius [m] + * \return true if the wheel radius was found; false otherwise + */ +static bool getWheelRadius(const boost::shared_ptr& wheel_link, double& wheel_radius) +{ + if(!isCylinder(wheel_link)) + { + ROS_ERROR_STREAM("Wheel link " << wheel_link->name << " is NOT modeled as a cylinder!"); + return false; + } + + wheel_radius = (static_cast(wheel_link->collision->geometry.get()))->radius; + return true; +} + +namespace diff_drive_controller{ + + DiffDriveController::DiffDriveController() + : open_loop_(false) + , command_struct_() + , wheel_separation_(0.0) + , wheel_radius_(0.0) + , wheel_separation_multiplier_(1.0) + , wheel_radius_multiplier_(1.0) + , cmd_vel_timeout_(0.5) + , base_frame_id_("base_link") + , enable_odom_tf_(true) + , wheel_joints_size_(0) + { + } + + 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 left_wheel_names, right_wheel_names; + if (!getWheelNames(controller_nh, "left_wheel", left_wheel_names) or + !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_ << "."); + + controller_nh.param("wheel_radius_multiplier", wheel_radius_multiplier_, wheel_radius_multiplier_); + ROS_INFO_STREAM_NAMED(name_, "Wheel radius will be multiplied by " + << wheel_radius_multiplier_ << "."); + + // 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("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("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/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("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/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 ); + + if (!setOdomParamsFromUrdf(root_nh, left_wheel_names[0], right_wheel_names[0])) + return false; + + setOdomPubFields(root_nh, controller_nh); + + // Get the joint object to use in the realtime loop + for (int 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); + + return true; + } + + void DiffDriveController::update(const ros::Time& time, const ros::Duration& period) + { + // COMPUTE AND PUBLISH ODOMETRY + if (open_loop_) + { + odometry_.updateOpenLoop(last_cmd_.lin, last_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, last_cmd_.lin, cmd_dt); + limiter_ang_.limit(curr_cmd.ang, last_cmd_.ang, cmd_dt); + last_cmd_ = curr_cmd; + + // Apply multipliers: + const double ws = wheel_separation_multiplier_ * wheel_separation_; + const double wr = wheel_radius_multiplier_ * wheel_radius_; + + // Compute wheels velocities: + const double vel_left = (curr_cmd.lin - curr_cmd.ang * ws / 2.0)/wr; + const double vel_right = (curr_cmd.lin + curr_cmd.ang * ws / 2.0)/wr; + + // 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); + } + } + + void DiffDriveController::starting(const ros::Time& time) + { + brake(); + + // Register starting time used to keep fixed rate + last_state_publish_time_ = 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()) + { + 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& 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(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) + { + // Parse robot description + const std::string model_param_name = "robot_description"; + bool res = root_nh.hasParam(model_param_name); + std::string robot_model_str=""; + if(!res || !root_nh.getParam(model_param_name,robot_model_str)) + { + ROS_ERROR_NAMED(name_, "Robot descripion couldn't be retrieved from param server."); + return false; + } + + boost::shared_ptr model(urdf::parseURDF(robot_model_str)); + + // Get wheel separation + boost::shared_ptr left_wheel_joint(model->getJoint(left_wheel_name)); + if(!left_wheel_joint) + { + ROS_ERROR_STREAM_NAMED(name_, left_wheel_name + << " couldn't be retrieved from model description"); + return false; + } + boost::shared_ptr right_wheel_joint(model->getJoint(right_wheel_name)); + if(!right_wheel_joint) + { + ROS_ERROR_STREAM_NAMED(name_, right_wheel_name + << " couldn't be retrieved from model description"); + return false; + } + + 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); + + // 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; + } + + // Set wheel params for the odometry computation + const double ws = wheel_separation_multiplier_ * wheel_separation_; + const double wr = wheel_radius_multiplier_ * wheel_radius_; + odometry_.setWheelParams(ws, wr); + ROS_INFO_STREAM_NAMED(name_, + "Odometry params : wheel separation " << ws + << ", wheel radius " << wr); + 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(controller_nh, "odom", 100)); + odom_pub_->msg_.header.frame_id = "odom"; + odom_pub_->msg_.child_frame_id = base_frame_id_; + odom_pub_->msg_.pose.pose.position.z = 0; + odom_pub_->msg_.pose.covariance = boost::assign::list_of + (static_cast(pose_cov_list[0])) (0) (0) (0) (0) (0) + (0) (static_cast(pose_cov_list[1])) (0) (0) (0) (0) + (0) (0) (static_cast(pose_cov_list[2])) (0) (0) (0) + (0) (0) (0) (static_cast(pose_cov_list[3])) (0) (0) + (0) (0) (0) (0) (static_cast(pose_cov_list[4])) (0) + (0) (0) (0) (0) (0) (static_cast(pose_cov_list[5])); + odom_pub_->msg_.twist.twist.linear.y = 0; + odom_pub_->msg_.twist.twist.linear.z = 0; + odom_pub_->msg_.twist.twist.angular.x = 0; + odom_pub_->msg_.twist.twist.angular.y = 0; + odom_pub_->msg_.twist.covariance = boost::assign::list_of + (static_cast(twist_cov_list[0])) (0) (0) (0) (0) (0) + (0) (static_cast(twist_cov_list[1])) (0) (0) (0) (0) + (0) (0) (static_cast(twist_cov_list[2])) (0) (0) (0) + (0) (0) (0) (static_cast(twist_cov_list[3])) (0) (0) + (0) (0) (0) (0) (static_cast(twist_cov_list[4])) (0) + (0) (0) (0) (0) (0) (static_cast(twist_cov_list[5])); + tf_odom_pub_.reset(new realtime_tools::RealtimePublisher(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"; + } + +} // namespace diff_drive_controller diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp new file mode 100644 index 000000000..499fe8756 --- /dev/null +++ b/mecanum_drive_controller/src/odometry.cpp @@ -0,0 +1,164 @@ +/********************************************************************* + * 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 + +#include + +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) + , 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_(boost::bind(&Odometry::integrateExact, this, _1, _2)) + { + } + + void Odometry::init(const ros::Time& time) + { + // Reset accumulators: + linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); + angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); + + // Reset timestamp: + 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 * wheel_radius_; + const double right_wheel_cur_pos = right_pos * wheel_radius_; + + /// Estimate velocity of wheels using old and current position: + const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_; + const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_; + + /// 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 wheel_radius) + { + wheel_separation_ = wheel_separation; + wheel_radius_ = wheel_radius; + } + + 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) < 10e-3) + 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)); + } + } + +} // namespace diff_drive_controller diff --git a/mecanum_drive_controller/src/speed_limiter.cpp b/mecanum_drive_controller/src/speed_limiter.cpp new file mode 100644 index 000000000..39e1d7b88 --- /dev/null +++ b/mecanum_drive_controller/src/speed_limiter.cpp @@ -0,0 +1,98 @@ +/********************************************************************* + * 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 + +#include + +template +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, + double min_velocity, + double max_velocity, + double min_acceleration, + double max_acceleration + ) + : has_velocity_limits(has_velocity_limits), + has_acceleration_limits(has_acceleration_limits), + min_velocity(min_velocity), + max_velocity(max_velocity), + min_acceleration(min_acceleration), + max_acceleration(max_acceleration) + { + } + + void SpeedLimiter::limit(double& v, double v0, double dt) + { + limit_velocity(v); + limit_acceleration(v, v0, dt); + } + + void SpeedLimiter::limit_velocity(double& v) + { + if (has_velocity_limits) + { + v = clamp(v, min_velocity, max_velocity); + } + } + + void SpeedLimiter::limit_acceleration(double& v, double v0, double dt) + { + if (has_acceleration_limits) + { + double dv = v - v0; + + const double dv_min = min_acceleration * dt; + const double dv_max = max_acceleration * dt; + + dv = clamp(dv, dv_min, dv_max); + + v = v0 + dv; + } + } + +} // namespace diff_drive_controller diff --git a/mecanum_drive_controller/test/diff_drive_bad_urdf.test b/mecanum_drive_controller/test/diff_drive_bad_urdf.test new file mode 100644 index 000000000..2932c6f6b --- /dev/null +++ b/mecanum_drive_controller/test/diff_drive_bad_urdf.test @@ -0,0 +1,17 @@ + + + + + + + + + + + + + diff --git a/mecanum_drive_controller/test/diff_drive_common.launch b/mecanum_drive_controller/test/diff_drive_common.launch new file mode 100644 index 000000000..b63aaa8d6 --- /dev/null +++ b/mecanum_drive_controller/test/diff_drive_common.launch @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + diff --git a/mecanum_drive_controller/test/diff_drive_controller.test b/mecanum_drive_controller/test/diff_drive_controller.test new file mode 100644 index 000000000..ceaaa46fd --- /dev/null +++ b/mecanum_drive_controller/test/diff_drive_controller.test @@ -0,0 +1,13 @@ + + + + + + + + + + diff --git a/mecanum_drive_controller/test/diff_drive_controller_limits.test b/mecanum_drive_controller/test/diff_drive_controller_limits.test new file mode 100644 index 000000000..42d6bcb09 --- /dev/null +++ b/mecanum_drive_controller/test/diff_drive_controller_limits.test @@ -0,0 +1,16 @@ + + + + + + + + + + + + + diff --git a/mecanum_drive_controller/test/diff_drive_fail_test.cpp b/mecanum_drive_controller/test/diff_drive_fail_test.cpp new file mode 100644 index 000000000..f12048800 --- /dev/null +++ b/mecanum_drive_controller/test/diff_drive_fail_test.cpp @@ -0,0 +1,57 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2013, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Paul Mathieu + +#include "test_common.h" + +// TEST CASES +TEST_F(DiffDriveControllerTest, testWrongJointName) +{ + // the controller should never be alive + int secs = 0; + while(!isControllerAlive() && secs < 5) + { + ros::Duration(1.0).sleep(); + secs++; + } + // give up and assume controller load failure after 5 secondds + 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; +} diff --git a/mecanum_drive_controller/test/diff_drive_limits_test.cpp b/mecanum_drive_controller/test/diff_drive_limits_test.cpp new file mode 100644 index 000000000..888ef5eb6 --- /dev/null +++ b/mecanum_drive_controller/test/diff_drive_limits_test.cpp @@ -0,0 +1,169 @@ +/////////////////////////////////////////////////////////////////////////////// +// 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, 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(new_odom.pose.pose.position.x - old_odom.pose.pose.position.x, 5.0 + POSITION_TOLERANCE); + EXPECT_LT(fabs(new_odom.pose.pose.position.y - old_odom.pose.pose.position.y), EPS); + + cmd_vel.linear.x = 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; +} diff --git a/mecanum_drive_controller/test/diff_drive_multipliers.test b/mecanum_drive_controller/test/diff_drive_multipliers.test new file mode 100644 index 000000000..560330ee2 --- /dev/null +++ b/mecanum_drive_controller/test/diff_drive_multipliers.test @@ -0,0 +1,16 @@ + + + + + + + + + + + + + diff --git a/mecanum_drive_controller/test/diff_drive_odom_tf.test b/mecanum_drive_controller/test/diff_drive_odom_tf.test new file mode 100644 index 000000000..e733666a7 --- /dev/null +++ b/mecanum_drive_controller/test/diff_drive_odom_tf.test @@ -0,0 +1,19 @@ + + + + + + + diffbot_controller: + enable_odom_tf: False + + + + + + + + diff --git a/mecanum_drive_controller/test/diff_drive_odom_tf_test.cpp b/mecanum_drive_controller/test/diff_drive_odom_tf_test.cpp new file mode 100644 index 000000000..41aae1b31 --- /dev/null +++ b/mecanum_drive_controller/test/diff_drive_odom_tf_test.cpp @@ -0,0 +1,59 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2014, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Bence Magyar + +#include "test_common.h" +#include + +// TEST CASES +TEST_F(DiffDriveControllerTest, testNoOdomFrame) +{ + // wait for ROS + while(!isControllerAlive()) + { + ros::Duration(0.1).sleep(); + } + // set up tf listener + tf::TransformListener listener; + ros::Duration(2.0).sleep(); + // check the odom frame doesn't exist + EXPECT_FALSE(listener.frameExists("odom")); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "diff_drive_odom_tf_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/mecanum_drive_controller/test/diff_drive_open_loop.test b/mecanum_drive_controller/test/diff_drive_open_loop.test new file mode 100644 index 000000000..31bf11532 --- /dev/null +++ b/mecanum_drive_controller/test/diff_drive_open_loop.test @@ -0,0 +1,16 @@ + + + + + + + + + + + + + diff --git a/mecanum_drive_controller/test/diff_drive_test.cpp b/mecanum_drive_controller/test/diff_drive_test.cpp new file mode 100644 index 000000000..8f402fb3b --- /dev/null +++ b/mecanum_drive_controller/test/diff_drive_test.cpp @@ -0,0 +1,151 @@ +/////////////////////////////////////////////////////////////////////////////// +// 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 + +// TEST CASES +TEST_F(DiffDriveControllerTest, testForward) +{ + // wait for ROS + while(!isControllerAlive()) + { + ros::Duration(0.1).sleep(); + } + // zero everything before test + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.angular.z = 0.0; + publish(cmd_vel); + ros::Duration(0.1).sleep(); + // get initial odom + nav_msgs::Odometry old_odom = getLastOdom(); + // send a velocity command of 0.1 m/s + cmd_vel.linear.x = 0.1; + publish(cmd_vel); + // wait for 10s + ros::Duration(10.0).sleep(); + + nav_msgs::Odometry new_odom = getLastOdom(); + + // check if the robot travelled 1 meter in x, changes in the other fields should be ~~0 + EXPECT_NEAR(fabs(new_odom.pose.pose.position.x - old_odom.pose.pose.position.x), 1.0, POSITION_TOLERANCE); + 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); + + // 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), 0.1, 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 + while(!isControllerAlive()) + { + ros::Duration(0.1).sleep(); + } + // zero everything before test + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.angular.z = 0.0; + publish(cmd_vel); + ros::Duration(0.1).sleep(); + // get initial odom + nav_msgs::Odometry old_odom = getLastOdom(); + // send a velocity command + cmd_vel.angular.z = M_PI/10.0; + 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); + + // convert to rpy and test that way + double roll_old, pitch_old, yaw_old; + double roll_new, pitch_new, yaw_new; + tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old); + tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new); + EXPECT_LT(fabs(roll_new - roll_old), EPS); + EXPECT_LT(fabs(pitch_new - pitch_old), EPS); + EXPECT_NEAR(fabs(yaw_new - yaw_old), M_PI, ORIENTATION_TOLERANCE); + + EXPECT_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), M_PI/10.0, EPS); +} + +TEST_F(DiffDriveControllerTest, testOdomFrame) +{ + // wait for ROS + while(!isControllerAlive()) + { + ros::Duration(0.1).sleep(); + } + // set up tf listener + tf::TransformListener listener; + ros::Duration(2.0).sleep(); + // check the odom frame exist + EXPECT_TRUE(listener.frameExists("odom")); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "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; +} diff --git a/mecanum_drive_controller/test/diff_drive_timeout.test b/mecanum_drive_controller/test/diff_drive_timeout.test new file mode 100644 index 000000000..4563132b3 --- /dev/null +++ b/mecanum_drive_controller/test/diff_drive_timeout.test @@ -0,0 +1,16 @@ + + + + + + + + + + + + + diff --git a/mecanum_drive_controller/test/diff_drive_timeout_test.cpp b/mecanum_drive_controller/test/diff_drive_timeout_test.cpp new file mode 100644 index 000000000..41a12bd79 --- /dev/null +++ b/mecanum_drive_controller/test/diff_drive_timeout_test.cpp @@ -0,0 +1,72 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2013, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Paul Mathieu + +#include "test_common.h" + +// TEST CASES +TEST_F(DiffDriveControllerTest, testTimeout) +{ + // wait for ROS + while(!isControllerAlive()) + { + ros::Duration(0.1).sleep(); + } + // zero everything before test + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.angular.z = 0.0; + publish(cmd_vel); + // give some time to the controller to react to the command + ros::Duration(0.1).sleep(); + // get initial odom + nav_msgs::Odometry old_odom = getLastOdom(); + // send a velocity command of 1 m/s + cmd_vel.linear.x = 1.0; + publish(cmd_vel); + // wait a bit + ros::Duration(3.0).sleep(); + + nav_msgs::Odometry new_odom = getLastOdom(); + + // check if the robot has stopped after 0.5s, thus covering less than 0.5s*1.0m.s-1 + some (big) tolerance + EXPECT_LT(fabs(new_odom.pose.pose.position.x - old_odom.pose.pose.position.x), 0.8); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "diff_drive_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/mecanum_drive_controller/test/diff_drive_wrong.test b/mecanum_drive_controller/test/diff_drive_wrong.test new file mode 100644 index 000000000..bb4164fae --- /dev/null +++ b/mecanum_drive_controller/test/diff_drive_wrong.test @@ -0,0 +1,16 @@ + + + + + + + + + + + + + diff --git a/mecanum_drive_controller/test/diffbot.cpp b/mecanum_drive_controller/test/diffbot.cpp new file mode 100644 index 000000000..60ae82551 --- /dev/null +++ b/mecanum_drive_controller/test/diffbot.cpp @@ -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. +////////////////////////////////////////////////////////////////////////////// + +// NOTE: The contents of this file have been taken largely from the ros_control wiki tutorials + +// ROS +#include + +// ros_control +#include + +#include "diffbot.h" + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "diffbot"); + ros::NodeHandle nh; + + Diffbot<> robot; + ROS_WARN_STREAM("period: " << robot.getPeriod().toSec()); + controller_manager::ControllerManager cm(&robot, nh); + + ros::Rate rate(1.0 / robot.getPeriod().toSec()); + ros::AsyncSpinner spinner(1); + spinner.start(); + while(ros::ok()) + { + robot.read(); + cm.update(robot.getTime(), robot.getPeriod()); + robot.write(); + rate.sleep(); + } + spinner.stop(); + + return 0; +} diff --git a/mecanum_drive_controller/test/diffbot.h b/mecanum_drive_controller/test/diffbot.h new file mode 100644 index 000000000..afa082c41 --- /dev/null +++ b/mecanum_drive_controller/test/diffbot.h @@ -0,0 +1,139 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2013, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +// NOTE: The contents of this file have been taken largely from the ros_control wiki tutorials + +// ROS +#include +#include +#include + +// ros_control +#include +#include +#include +#include +#include + +// NaN +#include + +// ostringstream +#include + +template +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() + { + 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()); + } + + void write() + { + 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::quiet_NaN()); + std::fill_n(vel_, NUM_JOINTS, std::numeric_limits::quiet_NaN()); + } + } + + 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_; +}; diff --git a/mecanum_drive_controller/test/diffbot.xacro b/mecanum_drive_controller/test/diffbot.xacro new file mode 100644 index 000000000..f2ace6cc4 --- /dev/null +++ b/mecanum_drive_controller/test/diffbot.xacro @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Green + + + + Gazebo/Purple + + + diff --git a/mecanum_drive_controller/test/diffbot_bad.xacro b/mecanum_drive_controller/test/diffbot_bad.xacro new file mode 100644 index 000000000..ec88c3890 --- /dev/null +++ b/mecanum_drive_controller/test/diffbot_bad.xacro @@ -0,0 +1,142 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + 1 + + + + + + 1 + 1 + + + + + Gazebo/Green + + + + Gazebo/Red + + + + Gazebo/Blue + + + + Gazebo/Purple + + + diff --git a/mecanum_drive_controller/test/diffbot_controllers.yaml b/mecanum_drive_controller/test/diffbot_controllers.yaml new file mode 100644 index 000000000..f89c38c37 --- /dev/null +++ b/mecanum_drive_controller/test/diffbot_controllers.yaml @@ -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 diff --git a/mecanum_drive_controller/test/diffbot_limits.yaml b/mecanum_drive_controller/test/diffbot_limits.yaml new file mode 100644 index 000000000..fda2cc5b7 --- /dev/null +++ b/mecanum_drive_controller/test/diffbot_limits.yaml @@ -0,0 +1,15 @@ +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 + angular: + z: + has_velocity_limits: true + max_velocity: 2.0 + has_acceleration_limits: true + max_acceleration: 2.0 diff --git a/mecanum_drive_controller/test/diffbot_multipliers.yaml b/mecanum_drive_controller/test/diffbot_multipliers.yaml new file mode 100644 index 000000000..d2ef5d5b8 --- /dev/null +++ b/mecanum_drive_controller/test/diffbot_multipliers.yaml @@ -0,0 +1,3 @@ +diffbot_controller: + wheel_separation_multiplier: 2.3 + wheel_radius_multiplier: 1.4 diff --git a/mecanum_drive_controller/test/diffbot_open_loop.yaml b/mecanum_drive_controller/test/diffbot_open_loop.yaml new file mode 100644 index 000000000..19224f3bc --- /dev/null +++ b/mecanum_drive_controller/test/diffbot_open_loop.yaml @@ -0,0 +1,2 @@ +diffbot_controller: + open_loop: true diff --git a/mecanum_drive_controller/test/diffbot_timeout.yaml b/mecanum_drive_controller/test/diffbot_timeout.yaml new file mode 100644 index 000000000..4ecfa1877 --- /dev/null +++ b/mecanum_drive_controller/test/diffbot_timeout.yaml @@ -0,0 +1,2 @@ +diffbot_controller: + cmd_vel_timeout: 0.5 diff --git a/mecanum_drive_controller/test/diffbot_wrong.yaml b/mecanum_drive_controller/test/diffbot_wrong.yaml new file mode 100644 index 000000000..516fd75c3 --- /dev/null +++ b/mecanum_drive_controller/test/diffbot_wrong.yaml @@ -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 diff --git a/mecanum_drive_controller/test/skid_steer_common.launch b/mecanum_drive_controller/test/skid_steer_common.launch new file mode 100644 index 000000000..fe7e94804 --- /dev/null +++ b/mecanum_drive_controller/test/skid_steer_common.launch @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + diff --git a/mecanum_drive_controller/test/skid_steer_controller.test b/mecanum_drive_controller/test/skid_steer_controller.test new file mode 100644 index 000000000..b89aab12b --- /dev/null +++ b/mecanum_drive_controller/test/skid_steer_controller.test @@ -0,0 +1,13 @@ + + + + + + + + + + diff --git a/mecanum_drive_controller/test/skid_steer_no_wheels.test b/mecanum_drive_controller/test/skid_steer_no_wheels.test new file mode 100644 index 000000000..710096049 --- /dev/null +++ b/mecanum_drive_controller/test/skid_steer_no_wheels.test @@ -0,0 +1,16 @@ + + + + + + + + + + + + + diff --git a/mecanum_drive_controller/test/skidsteerbot.cpp b/mecanum_drive_controller/test/skidsteerbot.cpp new file mode 100644 index 000000000..37ca8c3f4 --- /dev/null +++ b/mecanum_drive_controller/test/skidsteerbot.cpp @@ -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. +////////////////////////////////////////////////////////////////////////////// + +// NOTE: The contents of this file have been taken largely from the ros_control wiki tutorials + +// ROS +#include + +// ros_control +#include + +#include "diffbot.h" + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "skidsteerbot"); + ros::NodeHandle nh; + + Diffbot<6> robot; + ROS_WARN_STREAM("period: " << robot.getPeriod().toSec()); + controller_manager::ControllerManager cm(&robot, nh); + + ros::Rate rate(1.0 / robot.getPeriod().toSec()); + ros::AsyncSpinner spinner(1); + spinner.start(); + while(ros::ok()) + { + robot.read(); + cm.update(robot.getTime(), robot.getPeriod()); + robot.write(); + rate.sleep(); + } + spinner.stop(); + + return 0; +} diff --git a/mecanum_drive_controller/test/skidsteerbot.xacro b/mecanum_drive_controller/test/skidsteerbot.xacro new file mode 100644 index 000000000..657602655 --- /dev/null +++ b/mecanum_drive_controller/test/skidsteerbot.xacro @@ -0,0 +1,89 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Green + + + + Gazebo/Purple + + + diff --git a/mecanum_drive_controller/test/skidsteerbot_controllers.yaml b/mecanum_drive_controller/test/skidsteerbot_controllers.yaml new file mode 100644 index 000000000..ab4942480 --- /dev/null +++ b/mecanum_drive_controller/test/skidsteerbot_controllers.yaml @@ -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 diff --git a/mecanum_drive_controller/test/skidsteerbot_no_wheels.yaml b/mecanum_drive_controller/test/skidsteerbot_no_wheels.yaml new file mode 100644 index 000000000..37f0d99dd --- /dev/null +++ b/mecanum_drive_controller/test/skidsteerbot_no_wheels.yaml @@ -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 diff --git a/mecanum_drive_controller/test/test_common.h b/mecanum_drive_controller/test/test_common.h new file mode 100644 index 000000000..9738d9c88 --- /dev/null +++ b/mecanum_drive_controller/test/test_common.h @@ -0,0 +1,85 @@ +/////////////////////////////////////////////////////////////////////////////// +// 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 + +#include + +#include + +#include +#include +#include + +// 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 ORIENTATION_TOLERANCE = 0.03; // 0.57 degree precision + +class DiffDriveControllerTest : public ::testing::Test +{ +public: + + DiffDriveControllerTest() + : cmd_pub(nh.advertise("cmd_vel", 100)), + odom_sub(nh.subscribe("odom", 100, &DiffDriveControllerTest::odomCallback, this)) + { + } + + ~DiffDriveControllerTest() + { + odom_sub.shutdown(); + } + + nav_msgs::Odometry getLastOdom(){ return last_odom; } + void publish(geometry_msgs::Twist cmd_vel){ cmd_pub.publish(cmd_vel); } + bool isControllerAlive(){ return (odom_sub.getNumPublishers() > 0) && (cmd_pub.getNumSubscribers() > 0); } + +private: + ros::NodeHandle nh; + ros::Publisher cmd_pub; + ros::Subscriber odom_sub; + nav_msgs::Odometry last_odom; + + void odomCallback(const nav_msgs::Odometry& odom) + { + ROS_INFO_STREAM("Callback reveived: pos.x: " << odom.pose.pose.position.x + << ", orient.z: " << odom.pose.pose.orientation.z + << ", lin_est: " << odom.twist.twist.linear.x + << ", ang_est: " << odom.twist.twist.angular.z); + last_odom = odom; + } +}; + +inline tf::Quaternion tfQuatFromGeomQuat(const geometry_msgs::Quaternion& quat) +{ + return tf::Quaternion(quat.x, quat.y, quat.z, quat.w); +} + diff --git a/mecanum_drive_controller/test/view_diffbot.launch b/mecanum_drive_controller/test/view_diffbot.launch new file mode 100644 index 000000000..cdc362e93 --- /dev/null +++ b/mecanum_drive_controller/test/view_diffbot.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/mecanum_drive_controller/test/view_skidsteerbot.launch b/mecanum_drive_controller/test/view_skidsteerbot.launch new file mode 100644 index 000000000..66d8d5c2b --- /dev/null +++ b/mecanum_drive_controller/test/view_skidsteerbot.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/mecanum_drive_controller/test/wheel.xacro b/mecanum_drive_controller/test/wheel.xacro new file mode 100644 index 000000000..6fa1bd60c --- /dev/null +++ b/mecanum_drive_controller/test/wheel.xacro @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + 1 + + + + Gazebo/Red + + + From cbf051fa3077fc984e12fd56998f750bf44e8414 Mon Sep 17 00:00:00 2001 From: Antoine Rennuit Date: Mon, 27 Oct 2014 17:57:21 +0100 Subject: [PATCH 02/21] Got the mecanum_drive_controller compile (not tested yet and should not work). --- mecanum_drive_controller/CHANGELOG.rst | 34 ++------------ mecanum_drive_controller/CMakeLists.txt | 45 ++----------------- mecanum_drive_controller/README.md | 6 +-- .../diff_drive_controller_plugins.xml | 9 ---- .../mecanum_drive_controller.h} | 4 +- .../odometry.h | 0 .../speed_limiter.h | 0 .../mecanum_drive_controller_plugins.xml | 9 ++++ mecanum_drive_controller/package.xml | 25 +++-------- ...oller.cpp => mecanum_drive_controller.cpp} | 2 +- mecanum_drive_controller/src/odometry.cpp | 2 +- .../src/speed_limiter.cpp | 2 +- 12 files changed, 30 insertions(+), 108 deletions(-) delete mode 100644 mecanum_drive_controller/diff_drive_controller_plugins.xml rename mecanum_drive_controller/include/{diff_drive_controller/diff_drive_controller.h => mecanum_drive_controller/mecanum_drive_controller.h} (98%) rename mecanum_drive_controller/include/{diff_drive_controller => mecanum_drive_controller}/odometry.h (100%) rename mecanum_drive_controller/include/{diff_drive_controller => mecanum_drive_controller}/speed_limiter.h (100%) create mode 100644 mecanum_drive_controller/mecanum_drive_controller_plugins.xml rename mecanum_drive_controller/src/{diff_drive_controller.cpp => mecanum_drive_controller.cpp} (99%) diff --git a/mecanum_drive_controller/CHANGELOG.rst b/mecanum_drive_controller/CHANGELOG.rst index 2de77d980..e9e106ec3 100644 --- a/mecanum_drive_controller/CHANGELOG.rst +++ b/mecanum_drive_controller/CHANGELOG.rst @@ -1,35 +1,7 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package diff_drive_controller +Changelog for package mecanum_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -0.8.1 (2014-07-11) +0.0.1 (2014-10-27) ------------------ - -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. +* First version of the mecanum drive controller. diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt index fb63c04c1..3554fc0aa 100644 --- a/mecanum_drive_controller/CMakeLists.txt +++ b/mecanum_drive_controller/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(diff_drive_controller) +project(mecanum_drive_controller) find_package(catkin REQUIRED COMPONENTS controller_interface @@ -17,7 +17,7 @@ include_directories( include ${catkin_INCLUDE_DIRS} ) -add_library(${PROJECT_NAME} src/diff_drive_controller.cpp src/odometry.cpp src/speed_limiter.cpp) +add_library(${PROJECT_NAME} src/mecanum_drive_controller.cpp src/odometry.cpp src/speed_limiter.cpp) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) install(TARGETS ${PROJECT_NAME} @@ -26,44 +26,5 @@ install(TARGETS ${PROJECT_NAME} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -install(FILES diff_drive_controller_plugins.xml +install(FILES mecanum_drive_controller_plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - -if (CATKIN_ENABLE_TESTING) - find_package(catkin COMPONENTS rostest std_srvs controller_manager tf) - include_directories(include ${catkin_INCLUDE_DIRS}) - - add_executable(diffbot test/diffbot.cpp) - target_link_libraries(diffbot ${catkin_LIBRARIES}) - - add_executable(skidsteerbot test/skidsteerbot.cpp) - target_link_libraries(skidsteerbot ${catkin_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}) - 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}) - 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}) - add_rostest(test/diff_drive_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}) - add_rostest(test/diff_drive_bad_urdf.test) - 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}) - add_rostest(test/diff_drive_open_loop.test) - add_rostest(test/skid_steer_controller.test) - add_rostest(test/skid_steer_no_wheels.test) -endif() diff --git a/mecanum_drive_controller/README.md b/mecanum_drive_controller/README.md index 371baed1f..c488a081d 100644 --- a/mecanum_drive_controller/README.md +++ b/mecanum_drive_controller/README.md @@ -1,5 +1,5 @@ -## Diff Drive Controller ## +## Mecanum Drive Controller ## -Controller for a differential drive mobile base. +Controller for a mecanum drive mobile base. -Detailed user documentation can be found in the controller's [ROS wiki page](http://wiki.ros.org/diff_drive_controller). +The development of this controller was based on the differential drive controller called diff_drive_controller. diff --git a/mecanum_drive_controller/diff_drive_controller_plugins.xml b/mecanum_drive_controller/diff_drive_controller_plugins.xml deleted file mode 100644 index 3ec80bfa3..000000000 --- a/mecanum_drive_controller/diff_drive_controller_plugins.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - - The DiffDriveController tracks velocity commands. It expects 2 VelocityJointInterface type of hardware interfaces. - - - - diff --git a/mecanum_drive_controller/include/diff_drive_controller/diff_drive_controller.h b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h similarity index 98% rename from mecanum_drive_controller/include/diff_drive_controller/diff_drive_controller.h rename to mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h index 8392dd087..835fe612e 100644 --- a/mecanum_drive_controller/include/diff_drive_controller/diff_drive_controller.h +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h @@ -46,8 +46,8 @@ #include #include -#include -#include +#include +#include namespace diff_drive_controller{ diff --git a/mecanum_drive_controller/include/diff_drive_controller/odometry.h b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h similarity index 100% rename from mecanum_drive_controller/include/diff_drive_controller/odometry.h rename to mecanum_drive_controller/include/mecanum_drive_controller/odometry.h diff --git a/mecanum_drive_controller/include/diff_drive_controller/speed_limiter.h b/mecanum_drive_controller/include/mecanum_drive_controller/speed_limiter.h similarity index 100% rename from mecanum_drive_controller/include/diff_drive_controller/speed_limiter.h rename to mecanum_drive_controller/include/mecanum_drive_controller/speed_limiter.h diff --git a/mecanum_drive_controller/mecanum_drive_controller_plugins.xml b/mecanum_drive_controller/mecanum_drive_controller_plugins.xml new file mode 100644 index 000000000..78e1b277e --- /dev/null +++ b/mecanum_drive_controller/mecanum_drive_controller_plugins.xml @@ -0,0 +1,9 @@ + + + + + The MecanumDriveController tracks velocity commands. It 4 joints should have a VelocityJointInterface hardware interface. + + + + diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml index 9aa0f0b60..5e3394924 100644 --- a/mecanum_drive_controller/package.xml +++ b/mecanum_drive_controller/package.xml @@ -1,16 +1,12 @@ - diff_drive_controller - 0.8.1 - Controller for a differential drive mobile base. - Bence Magyar + mecanum_drive_controller + 0.0.1 + Controller for a mecanum drive mobile base. + Antoine Rennuit - BSD + MIT - https://github.com/ros-controls/ros_controllers/wiki - https://github.com/ros-controls/ros_controllers/issues - https://github.com/ros-controls/ros_controllers - - Bence Magyar + Antoine Rennuit catkin @@ -26,14 +22,7 @@ tf urdf - - rostest - - std_srvs - controller_manager - xacro - - + diff --git a/mecanum_drive_controller/src/diff_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp similarity index 99% rename from mecanum_drive_controller/src/diff_drive_controller.cpp rename to mecanum_drive_controller/src/mecanum_drive_controller.cpp index 49f6444f7..b383fe95d 100644 --- a/mecanum_drive_controller/src/diff_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -42,7 +42,7 @@ #include -#include +#include static double euclideanOfVectors(const urdf::Vector3& vec1, const urdf::Vector3& vec2) { diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index 499fe8756..d88cfa2d0 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -39,7 +39,7 @@ * Author: Paul Mathieu */ -#include +#include #include diff --git a/mecanum_drive_controller/src/speed_limiter.cpp b/mecanum_drive_controller/src/speed_limiter.cpp index 39e1d7b88..1392be2d1 100644 --- a/mecanum_drive_controller/src/speed_limiter.cpp +++ b/mecanum_drive_controller/src/speed_limiter.cpp @@ -38,7 +38,7 @@ #include -#include +#include template T clamp(T x, T min, T max) From 613f9e55347ef912ed8ebbf1d38a2e8df3262d51 Mon Sep 17 00:00:00 2001 From: Antoine Rennuit Date: Mon, 27 Oct 2014 18:05:28 +0100 Subject: [PATCH 03/21] Some class and namespace renaming --- .../mecanum_drive_controller.h | 10 +++---- .../mecanum_drive_controller/odometry.h | 4 +-- .../mecanum_drive_controller/speed_limiter.h | 4 +-- .../src/mecanum_drive_controller.cpp | 26 +++++++++---------- mecanum_drive_controller/src/odometry.cpp | 4 +-- .../src/speed_limiter.cpp | 4 +-- 6 files changed, 26 insertions(+), 26 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h index 835fe612e..fccc8e411 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h @@ -49,7 +49,7 @@ #include #include -namespace diff_drive_controller{ +namespace mecanum_drive_controller{ /** * This class makes some assumptions on the model of the robot: @@ -60,11 +60,11 @@ namespace diff_drive_controller{ * - a wheel collision geometry is a cylinder in the urdf * - a wheel joint frame center's vertical projection on the floor must lie within the contact patch */ - class DiffDriveController + class MecanumDriveController : public controller_interface::Controller { public: - DiffDriveController(); + MecanumDriveController(); /** * \brief Initialize controller @@ -196,5 +196,5 @@ namespace diff_drive_controller{ }; - PLUGINLIB_EXPORT_CLASS(diff_drive_controller::DiffDriveController, controller_interface::ControllerBase); -} // namespace diff_drive_controller + PLUGINLIB_EXPORT_CLASS(mecanum_drive_controller::MecanumDriveController, controller_interface::ControllerBase) +} // namespace mecanum_drive_controller diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h index 7c665b537..a58f0eae5 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h @@ -48,7 +48,7 @@ #include #include -namespace diff_drive_controller +namespace mecanum_drive_controller { namespace bacc = boost::accumulators; @@ -194,6 +194,6 @@ namespace diff_drive_controller /// Integration funcion, used to integrate the odometry: IntegrationFunction integrate_fun_; }; -} +} // namespace mecanum_drive_controller #endif /* ODOMETRY_H_ */ diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/speed_limiter.h b/mecanum_drive_controller/include/mecanum_drive_controller/speed_limiter.h index aac881207..407451cdf 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/speed_limiter.h +++ b/mecanum_drive_controller/include/mecanum_drive_controller/speed_limiter.h @@ -39,7 +39,7 @@ #ifndef SPEED_LIMITER_H #define SPEED_LIMITER_H -namespace diff_drive_controller +namespace mecanum_drive_controller { class SpeedLimiter @@ -100,6 +100,6 @@ namespace diff_drive_controller double max_acceleration; }; -} // namespace diff_drive_controller +} // namespace mecanum_drive_controller #endif // SPEED_LIMITER_H diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index b383fe95d..3c4665d93 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -103,9 +103,9 @@ static bool getWheelRadius(const boost::shared_ptr& wheel_link return true; } -namespace diff_drive_controller{ +namespace mecanum_drive_controller{ - DiffDriveController::DiffDriveController() + MecanumDriveController::MecanumDriveController() : open_loop_(false) , command_struct_() , wheel_separation_(0.0) @@ -119,7 +119,7 @@ namespace diff_drive_controller{ { } - bool DiffDriveController::init(hardware_interface::VelocityJointInterface* hw, + bool MecanumDriveController::init(hardware_interface::VelocityJointInterface* hw, ros::NodeHandle& root_nh, ros::NodeHandle &controller_nh) { @@ -208,12 +208,12 @@ namespace diff_drive_controller{ right_wheel_joints_[i] = hw->getHandle(right_wheel_names[i]); // throws on failure } - sub_command_ = controller_nh.subscribe("cmd_vel", 1, &DiffDriveController::cmdVelCallback, this); + sub_command_ = controller_nh.subscribe("cmd_vel", 1, &MecanumDriveController::cmdVelCallback, this); return true; } - void DiffDriveController::update(const ros::Time& time, const ros::Duration& period) + void MecanumDriveController::update(const ros::Time& time, const ros::Duration& period) { // COMPUTE AND PUBLISH ODOMETRY if (open_loop_) @@ -307,7 +307,7 @@ namespace diff_drive_controller{ } } - void DiffDriveController::starting(const ros::Time& time) + void MecanumDriveController::starting(const ros::Time& time) { brake(); @@ -317,12 +317,12 @@ namespace diff_drive_controller{ odometry_.init(time); } - void DiffDriveController::stopping(const ros::Time& time) + void MecanumDriveController::stopping(const ros::Time& time) { brake(); } - void DiffDriveController::brake() + void MecanumDriveController::brake() { const double vel = 0.0; for (size_t i = 0; i < wheel_joints_size_; ++i) @@ -332,7 +332,7 @@ namespace diff_drive_controller{ } } - void DiffDriveController::cmdVelCallback(const geometry_msgs::Twist& command) + void MecanumDriveController::cmdVelCallback(const geometry_msgs::Twist& command) { if(isRunning()) { @@ -352,7 +352,7 @@ namespace diff_drive_controller{ } } - bool DiffDriveController::getWheelNames(ros::NodeHandle& controller_nh, + bool MecanumDriveController::getWheelNames(ros::NodeHandle& controller_nh, const std::string& wheel_param, std::vector& wheel_names) { @@ -405,7 +405,7 @@ namespace diff_drive_controller{ return true; } - bool DiffDriveController::setOdomParamsFromUrdf(ros::NodeHandle& root_nh, + bool MecanumDriveController::setOdomParamsFromUrdf(ros::NodeHandle& root_nh, const std::string& left_wheel_name, const std::string& right_wheel_name) { @@ -464,7 +464,7 @@ namespace diff_drive_controller{ return true; } - void DiffDriveController::setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) + void MecanumDriveController::setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) { // Get and check params for covariances XmlRpc::XmlRpcValue pose_cov_list; @@ -511,4 +511,4 @@ namespace diff_drive_controller{ tf_odom_pub_->msg_.transforms[0].header.frame_id = "odom"; } -} // namespace diff_drive_controller +} // namespace mecanum_drive_controller diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index d88cfa2d0..286a756dd 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -43,7 +43,7 @@ #include -namespace diff_drive_controller +namespace mecanum_drive_controller { namespace bacc = boost::accumulators; @@ -161,4 +161,4 @@ namespace diff_drive_controller } } -} // namespace diff_drive_controller +} // namespace mecanum_drive_controller diff --git a/mecanum_drive_controller/src/speed_limiter.cpp b/mecanum_drive_controller/src/speed_limiter.cpp index 1392be2d1..1eda26930 100644 --- a/mecanum_drive_controller/src/speed_limiter.cpp +++ b/mecanum_drive_controller/src/speed_limiter.cpp @@ -46,7 +46,7 @@ T clamp(T x, T min, T max) return std::min(std::max(min, x), max); } -namespace diff_drive_controller +namespace mecanum_drive_controller { SpeedLimiter::SpeedLimiter( @@ -95,4 +95,4 @@ namespace diff_drive_controller } } -} // namespace diff_drive_controller +} // namespace mecanum_drive_controller From e16e2ea5e7f0b814d43bf881a21352f30dab8819 Mon Sep 17 00:00:00 2001 From: Antoine Rennuit Date: Tue, 28 Oct 2014 17:31:16 +0100 Subject: [PATCH 04/21] Had mecanum_drive_controller compile. This lead to 2 modifications: * wheel radius can now be retrieved from spheres (as well as cylinders) * the roller's radius is used rather than the wheel's (need an option for this) --- .../src/mecanum_drive_controller.cpp | 26 +++++++++++++------ 1 file changed, 18 insertions(+), 8 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 3c4665d93..c7109a520 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -52,11 +52,11 @@ static double euclideanOfVectors(const urdf::Vector3& vec1, const urdf::Vector3& } /* - * \brief Check if the link is modeled as a cylinder + * \brief Check if the link is modeled as a cylinder or a sphere * \param link Link - * \return true if the link is modeled as a Cylinder; false otherwise + * \return true if the link is modeled as a Cylinder or Sphere; false otherwise */ -static bool isCylinder(const boost::shared_ptr& link) +static bool isCylinderOrSphere(const boost::shared_ptr& link) { if(!link) { @@ -76,9 +76,9 @@ static bool isCylinder(const boost::shared_ptr& link) return false; } - if(link->collision->geometry->type != urdf::Geometry::CYLINDER) + if(link->collision->geometry->type != urdf::Geometry::CYLINDER && link->collision->geometry->type != urdf::Geometry::SPHERE) { - ROS_ERROR_STREAM("Link " << link->name << " does not have cylinder geometry"); + ROS_ERROR_STREAM("Link " << link->name << " does not have cylinder nor sphere geometry"); return false; } @@ -93,13 +93,19 @@ static bool isCylinder(const boost::shared_ptr& link) */ static bool getWheelRadius(const boost::shared_ptr& wheel_link, double& wheel_radius) { - if(!isCylinder(wheel_link)) + if(!isCylinderOrSphere(wheel_link)) { ROS_ERROR_STREAM("Wheel link " << wheel_link->name << " is NOT modeled as a cylinder!"); return false; } - wheel_radius = (static_cast(wheel_link->collision->geometry.get()))->radius; + if (wheel_link->collision->geometry->type == urdf::Geometry::CYLINDER) + wheel_radius = (static_cast(wheel_link->collision->geometry.get()))->radius; + else + wheel_radius = (static_cast(wheel_link->collision->geometry.get()))->radius; + + ROS_INFO_STREAM(" radius: " << wheel_radius); + return true; } @@ -447,8 +453,12 @@ namespace mecanum_drive_controller{ wheel_separation_ = euclideanOfVectors(left_wheel_joint->parent_to_joint_origin_transform.position, right_wheel_joint->parent_to_joint_origin_transform.position); + const boost::shared_ptr& wheel_link = model->getLink(left_wheel_joint->child_link_name); + const boost::shared_ptr& roller_joint = wheel_link->child_joints[0]; + const boost::shared_ptr& roller_link = model->getLink(roller_joint->child_link_name); + // Get wheel radius - if(!getWheelRadius(model->getLink(left_wheel_joint->child_link_name), wheel_radius_)) + if(!getWheelRadius(roller_link, wheel_radius_)) { ROS_ERROR_STREAM_NAMED(name_, "Couldn't retrieve " << left_wheel_name << " wheel radius"); return false; From d01822e3319f13e1f11e673f68736c2a9e95f817 Mon Sep 17 00:00:00 2001 From: Antoine Rennuit Date: Tue, 28 Oct 2014 17:33:12 +0100 Subject: [PATCH 05/21] Added a second linearY parameter along linearX (a differential drive can only take a n X desired speed as it cannot move sideways instantaneously speaking). This second parameter is not used yet. --- .../mecanum_drive_controller.h | 8 ++-- .../mecanum_drive_controller/odometry.h | 33 ++++++++++----- .../src/mecanum_drive_controller.cpp | 35 +++++++++------- mecanum_drive_controller/src/odometry.cpp | 41 +++++++++++-------- 4 files changed, 72 insertions(+), 45 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h index fccc8e411..3c201240e 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h @@ -110,11 +110,12 @@ namespace mecanum_drive_controller{ /// Velocity command related: struct Commands { - double lin; + double linX; + double linY; double ang; ros::Time stamp; - Commands() : lin(0.0), ang(0.0), stamp(0.0) {} + Commands() : linX(0.0), linY(0.0), ang(0.0), stamp(0.0) {} }; realtime_tools::RealtimeBuffer command_; Commands command_struct_; @@ -150,7 +151,8 @@ namespace mecanum_drive_controller{ // Speed limiters: Commands last_cmd_; - SpeedLimiter limiter_lin_; + SpeedLimiter limiter_linX_; + SpeedLimiter limiter_linY_; SpeedLimiter limiter_ang_; private: diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h index a58f0eae5..9fc9c080a 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h @@ -61,7 +61,7 @@ namespace mecanum_drive_controller public: /// Integration function, used to integrate the odometry: - typedef boost::function IntegrationFunction; + typedef boost::function IntegrationFunction; /** * \brief Constructor @@ -88,11 +88,11 @@ namespace mecanum_drive_controller /** * \brief Updates the odometry class with latest velocity command - * \param linear Linear velocity [m/s] + * \param linearX Linear velocity [m/s] * \param angular Angular velocity [rad/s] * \param time Current time */ - void updateOpenLoop(double linear, double angular, const ros::Time &time); + void updateOpenLoop(double linearX, double linearY, double angular, const ros::Time &time); /** * \brief heading getter @@ -122,12 +122,21 @@ namespace mecanum_drive_controller } /** - * \brief linear velocity getter - * \return linear velocity [m/s] + * \brief linearX velocity getter + * \return linearX velocity [m/s] */ - double getLinear() const + double getLinearX() const { - return linear_; + return linearX_; + } + + /** + * \brief linearY velocity getter + * \return linearY velocity [m/s] + */ + double getLinearY() const + { + return linearY_; } /** @@ -157,14 +166,14 @@ namespace mecanum_drive_controller * \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); + void integrateRungeKutta2(double linearX, 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); + void integrateExact(double linearX, double linearY, double angular); /// Current timestamp: ros::Time timestamp_; @@ -175,7 +184,8 @@ namespace mecanum_drive_controller double heading_; // [rad] /// Current velocity: - double linear_; // [m/s] + double linearX_; // [m/s] + double linearY_; // [m/s] double angular_; // [rad/s] /// Wheel kinematic parameters [m]: @@ -188,7 +198,8 @@ namespace mecanum_drive_controller /// Rolling mean accumulators for the linar and angular velocities: size_t velocity_rolling_window_size_; - RollingMeanAcc linear_acc_; + RollingMeanAcc linearX_acc_; + RollingMeanAcc linearY_acc_; RollingMeanAcc angular_acc_; /// Integration funcion, used to integrate the odometry: diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index c7109a520..d2ea060f5 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -185,12 +185,12 @@ namespace mecanum_drive_controller{ 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/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/has_velocity_limits" , limiter_linX_.has_velocity_limits , limiter_linX_.has_velocity_limits ); + controller_nh.param("linear/x/has_acceleration_limits", limiter_linX_.has_acceleration_limits, limiter_linX_.has_acceleration_limits); + controller_nh.param("linear/x/max_velocity" , limiter_linX_.max_velocity , limiter_linX_.max_velocity ); + controller_nh.param("linear/x/min_velocity" , limiter_linX_.min_velocity , -limiter_linX_.max_velocity ); + controller_nh.param("linear/x/max_acceleration" , limiter_linX_.max_acceleration , limiter_linX_.max_acceleration ); + controller_nh.param("linear/x/min_acceleration" , limiter_linX_.min_acceleration , -limiter_linX_.max_acceleration ); 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); @@ -224,7 +224,7 @@ namespace mecanum_drive_controller{ // COMPUTE AND PUBLISH ODOMETRY if (open_loop_) { - odometry_.updateOpenLoop(last_cmd_.lin, last_cmd_.ang, time); + odometry_.updateOpenLoop(last_cmd_.linX, last_cmd_.linY, last_cmd_.ang, time); } else { @@ -262,7 +262,8 @@ namespace mecanum_drive_controller{ 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.linear.x = odometry_.getLinearX(); + odom_pub_->msg_.twist.twist.linear.y = odometry_.getLinearY(); odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular(); odom_pub_->unlockAndPublish(); } @@ -287,13 +288,15 @@ namespace mecanum_drive_controller{ // Brake if cmd_vel has timeout: if (dt > cmd_vel_timeout_) { - curr_cmd.lin = 0.0; + curr_cmd.linX = 0.0; + curr_cmd.linY = 0.0; curr_cmd.ang = 0.0; } // Limit velocities and accelerations: const double cmd_dt(period.toSec()); - limiter_lin_.limit(curr_cmd.lin, last_cmd_.lin, cmd_dt); + limiter_linX_.limit(curr_cmd.linX, last_cmd_.linX, cmd_dt); + limiter_linY_.limit(curr_cmd.linY, last_cmd_.linY, cmd_dt); limiter_ang_.limit(curr_cmd.ang, last_cmd_.ang, cmd_dt); last_cmd_ = curr_cmd; @@ -302,8 +305,8 @@ namespace mecanum_drive_controller{ const double wr = wheel_radius_multiplier_ * wheel_radius_; // Compute wheels velocities: - const double vel_left = (curr_cmd.lin - curr_cmd.ang * ws / 2.0)/wr; - const double vel_right = (curr_cmd.lin + curr_cmd.ang * ws / 2.0)/wr; + const double vel_left = (curr_cmd.linX - curr_cmd.ang * ws / 2.0)/wr; + const double vel_right = (curr_cmd.linX + curr_cmd.ang * ws / 2.0)/wr; // Set wheels velocities: for (size_t i = 0; i < wheel_joints_size_; ++i) @@ -343,13 +346,15 @@ namespace mecanum_drive_controller{ if(isRunning()) { command_struct_.ang = command.angular.z; - command_struct_.lin = command.linear.x; + command_struct_.linX = command.linear.x; + command_struct_.linY = command.linear.y; 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 << ", " + << "Lin: " << command_struct_.linX << ", " + << "Lin: " << command_struct_.linY << ", " << "Stamp: " << command_struct_.stamp); } else @@ -453,6 +458,8 @@ namespace mecanum_drive_controller{ wheel_separation_ = euclideanOfVectors(left_wheel_joint->parent_to_joint_origin_transform.position, right_wheel_joint->parent_to_joint_origin_transform.position); + // DEBUG. + // TODO: add param to retrieve wheel_link or roller_link. const boost::shared_ptr& wheel_link = model->getLink(left_wheel_joint->child_link_name); const boost::shared_ptr& roller_joint = wheel_link->child_joints[0]; const boost::shared_ptr& roller_link = model->getLink(roller_joint->child_link_name); diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index 286a756dd..5e778dd7a 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -52,23 +52,26 @@ namespace mecanum_drive_controller , x_(0.0) , y_(0.0) , heading_(0.0) - , linear_(0.0) + , linearX_(0.0) + , linearY_(0.0) , angular_(0.0) , wheel_separation_(0.0) , 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) + , linearX_acc_(RollingWindow::window_size = velocity_rolling_window_size) + , linearY_acc_(RollingWindow::window_size = velocity_rolling_window_size) , angular_acc_(RollingWindow::window_size = velocity_rolling_window_size) - , integrate_fun_(boost::bind(&Odometry::integrateExact, this, _1, _2)) + , integrate_fun_(boost::bind(&Odometry::integrateExact, this, _1, _2, _3)) { } void Odometry::init(const ros::Time& time) { // Reset accumulators: - linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); + linearX_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); + linearY_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); // Reset timestamp: @@ -90,11 +93,12 @@ namespace mecanum_drive_controller 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 linearX = (right_wheel_est_vel + left_wheel_est_vel) * 0.5 ; + const double linearY = 0.0; const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_; /// Integrate odometry: - integrate_fun_(linear, angular); + integrate_fun_(linearX, linearY, angular); /// We cannot estimate the speed with very small time intervals: const double dt = (time - timestamp_).toSec(); @@ -104,25 +108,28 @@ namespace mecanum_drive_controller timestamp_ = time; /// Estimate speeds using a rolling mean to filter them out: - linear_acc_(linear/dt); + linearX_acc_(linearX/dt); + linearY_acc_(linearY/dt); angular_acc_(angular/dt); - linear_ = bacc::rolling_mean(linear_acc_); + linearX_ = bacc::rolling_mean(linearX_acc_); + linearY_ = bacc::rolling_mean(linearY_acc_); angular_ = bacc::rolling_mean(angular_acc_); return true; } - void Odometry::updateOpenLoop(double linear, double angular, const ros::Time &time) + void Odometry::updateOpenLoop(double linearX, double linearY, double angular, const ros::Time &time) { /// Save last linear and angular velocity: - linear_ = linear; + linearX_ = linearX; + linearY_ = linearY; angular_ = angular; /// Integrate odometry: const double dt = (time - timestamp_).toSec(); timestamp_ = time; - integrate_fun_(linear * dt, angular * dt); + integrate_fun_(linearX * dt, linearY * dt, angular * dt); } void Odometry::setWheelParams(double wheel_separation, double wheel_radius) @@ -131,13 +138,13 @@ namespace mecanum_drive_controller wheel_radius_ = wheel_radius; } - void Odometry::integrateRungeKutta2(double linear, double angular) + void Odometry::integrateRungeKutta2(double linearX, double angular) { const double direction = heading_ + angular * 0.5; /// Runge-Kutta 2nd order integration: - x_ += linear * cos(direction); - y_ += linear * sin(direction); + x_ += linearX * cos(direction); + y_ += linearX * sin(direction); heading_ += angular; } @@ -146,15 +153,15 @@ namespace mecanum_drive_controller * \param linear * \param angular */ - void Odometry::integrateExact(double linear, double angular) + void Odometry::integrateExact(double linearX, double linearY, double angular) { if(fabs(angular) < 10e-3) - integrateRungeKutta2(linear, angular); + integrateRungeKutta2(linearX, angular); else { /// Exact integration (should solve problems when angular is zero): const double heading_old = heading_; - const double r = linear/angular; + const double r = linearX/angular; heading_ += angular; x_ += r * (sin(heading_) - sin(heading_old)); y_ += -r * (cos(heading_) - cos(heading_old)); From 7a37be1d6d6e24e99c511a11bb24fed77f2e054c Mon Sep 17 00:00:00 2001 From: Antoine Rennuit Date: Tue, 28 Oct 2014 18:10:38 +0100 Subject: [PATCH 06/21] Parameters were added to limit elocity on the y direction (it was already done for the x direction and the rotation). --- mecanum_drive_controller/src/mecanum_drive_controller.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index d2ea060f5..fc7e3cb0f 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -192,6 +192,13 @@ namespace mecanum_drive_controller{ controller_nh.param("linear/x/max_acceleration" , limiter_linX_.max_acceleration , limiter_linX_.max_acceleration ); controller_nh.param("linear/x/min_acceleration" , limiter_linX_.min_acceleration , -limiter_linX_.max_acceleration ); + controller_nh.param("linear/y/has_velocity_limits" , limiter_linY_.has_velocity_limits , limiter_linY_.has_velocity_limits ); + controller_nh.param("linear/y/has_acceleration_limits", limiter_linY_.has_acceleration_limits, limiter_linY_.has_acceleration_limits); + controller_nh.param("linear/y/max_velocity" , limiter_linY_.max_velocity , limiter_linY_.max_velocity ); + controller_nh.param("linear/y/min_velocity" , limiter_linY_.min_velocity , -limiter_linY_.max_velocity ); + controller_nh.param("linear/y/max_acceleration" , limiter_linY_.max_acceleration , limiter_linY_.max_acceleration ); + controller_nh.param("linear/y/min_acceleration" , limiter_linY_.min_acceleration , -limiter_linY_.max_acceleration ); + 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/max_velocity" , limiter_ang_.max_velocity , limiter_ang_.max_velocity ); From 34d0d217771e6f8b30f5801ca2f7728b5801867b Mon Sep 17 00:00:00 2001 From: Antoine Rennuit Date: Tue, 28 Oct 2014 18:10:38 +0100 Subject: [PATCH 07/21] A parameter was added to check whether the setup uses simulated mecanum wheels using realigned rollers. --- .../mecanum_drive_controller.h | 1 + .../src/mecanum_drive_controller.cpp | 29 +++++++++++++++---- 2 files changed, 24 insertions(+), 6 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h index 3c201240e..a1ebaf819 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h @@ -131,6 +131,7 @@ namespace mecanum_drive_controller{ double wheel_separation_; /// Wheel radius (assuming it's the same for the left and right wheels): + bool use_realigned_roller_joints_; double wheel_radius_; /// Wheel separation and radius calibration multipliers: diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index fc7e3cb0f..83e8b40d6 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -115,6 +115,7 @@ namespace mecanum_drive_controller{ : open_loop_(false) , command_struct_() , wheel_separation_(0.0) + , use_realigned_roller_joints_(false) , wheel_radius_(0.0) , wheel_separation_multiplier_(1.0) , wheel_radius_multiplier_(1.0) @@ -134,6 +135,9 @@ namespace mecanum_drive_controller{ name_ = complete_ns.substr(id + 1); // Get joint names from the parameter server + controller_nh.param("use_realigned_roller_joints", use_realigned_roller_joints_, false); + ROS_INFO_STREAM_NAMED(name_, "Use the roller's radius rather than the wheel's: " << use_realigned_roller_joints_ << "."); + std::vector left_wheel_names, right_wheel_names; if (!getWheelNames(controller_nh, "left_wheel", left_wheel_names) or !getWheelNames(controller_nh, "right_wheel", right_wheel_names)) @@ -465,14 +469,27 @@ namespace mecanum_drive_controller{ wheel_separation_ = euclideanOfVectors(left_wheel_joint->parent_to_joint_origin_transform.position, right_wheel_joint->parent_to_joint_origin_transform.position); - // DEBUG. - // TODO: add param to retrieve wheel_link or roller_link. - const boost::shared_ptr& wheel_link = model->getLink(left_wheel_joint->child_link_name); - const boost::shared_ptr& roller_joint = wheel_link->child_joints[0]; - const boost::shared_ptr& roller_link = model->getLink(roller_joint->child_link_name); + boost::shared_ptr radius_link = model->getLink(left_wheel_joint->child_link_name); + if (use_realigned_roller_joints_) + { + // This mode is used when the mecanum wheels are simulated and we use realigned rollers to mimic mecanum wheels. + const boost::shared_ptr& roller_joint = radius_link->child_joints[0]; + if(!roller_joint) + { + ROS_ERROR_STREAM_NAMED(name_, "No roller joint could be retrieved for wheel : " << left_wheel_joint->child_link_name << ". Are you sure mecanum wheels are simulated using realigned rollers?"); + return false; + } + + radius_link = model->getLink(roller_joint->child_link_name); + if(!radius_link) + { + ROS_ERROR_STREAM_NAMED(name_, "No roller link could be retrieved for wheel : " << left_wheel_joint->child_link_name << ". Are you sure mecanum wheels are simulated using realigned rollers?"); + return false; + } + } // Get wheel radius - if(!getWheelRadius(roller_link, wheel_radius_)) + if(!getWheelRadius(radius_link, wheel_radius_)) { ROS_ERROR_STREAM_NAMED(name_, "Couldn't retrieve " << left_wheel_name << " wheel radius"); return false; From 8ab247d7496cc9d8ff330d4d0113bd420218d915 Mon Sep 17 00:00:00 2001 From: Antoine Rennuit Date: Wed, 29 Oct 2014 15:57:22 +0100 Subject: [PATCH 08/21] First version of the mecanum wheels IK in (rough test ok). The twist which is to be provided is a body velocity, not a space velocity: I need to check whether this is standard or not. --- mecanum_drive_controller/CMakeLists.txt | 17 +- .../mecanum_drive_controller.h | 48 ++- .../mecanum_drive_controller/odometry.h | 36 +- .../src/mecanum_drive_controller.cpp | 356 ++++++++---------- mecanum_drive_controller/src/odometry.cpp | 81 ++-- 5 files changed, 237 insertions(+), 301 deletions(-) diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt index 3554fc0aa..0d1a4c88c 100644 --- a/mecanum_drive_controller/CMakeLists.txt +++ b/mecanum_drive_controller/CMakeLists.txt @@ -8,6 +8,21 @@ find_package(catkin REQUIRED COMPONENTS tf nav_msgs) +################################################ +# List sources. +################################################ +set(${PROJECT_NAME}_headers + include/mecanum_drive_controller/mecanum_drive_controller.h + include/mecanum_drive_controller/odometry.h + include/mecanum_drive_controller/speed_limiter.h +) + +set(${PROJECT_NAME}_sources + src/mecanum_drive_controller.cpp + src/odometry.cpp + src/speed_limiter.cpp +) + catkin_package( INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} @@ -17,7 +32,7 @@ include_directories( include ${catkin_INCLUDE_DIRS} ) -add_library(${PROJECT_NAME} src/mecanum_drive_controller.cpp src/odometry.cpp src/speed_limiter.cpp) +add_library(${PROJECT_NAME} ${${PROJECT_NAME}_headers} ${${PROJECT_NAME}_sources}) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) install(TARGETS ${PROJECT_NAME} diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h index a1ebaf819..28f9343b9 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h @@ -104,8 +104,10 @@ namespace mecanum_drive_controller{ bool open_loop_; /// Hardware handles: - std::vector left_wheel_joints_; - std::vector right_wheel_joints_; + hardware_interface::JointHandle wheel0_jointHandle_; + hardware_interface::JointHandle wheel1_jointHandle_; + hardware_interface::JointHandle wheel2_jointHandle_; + hardware_interface::JointHandle wheel3_jointHandle_; /// Velocity command related: struct Commands @@ -127,16 +129,10 @@ namespace mecanum_drive_controller{ Odometry odometry_; geometry_msgs::TransformStamped odom_frame_; - /// 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): bool use_realigned_roller_joints_; - double wheel_radius_; - - /// Wheel separation and radius calibration multipliers: - double wheel_separation_multiplier_; - double wheel_radius_multiplier_; + double wheels_k_; // wheels geometric param used in mecanum wheels' ik + double wheels_radius_; /// Timeout to consider cmd_vel commands old: double cmd_vel_timeout_; @@ -169,26 +165,26 @@ namespace mecanum_drive_controller{ 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 + * \brief Sets odometry parameters from the URDF, i.e. the wheel radius and separation + * \param root_nh Root node handle + * \param wheel0_name Name of wheel0 joint + * \param wheel1_name Name of wheel1 joint + * \param wheel2_name Name of wheel2 joint + * \param wheel3_name Name of wheel3 joint */ - bool getWheelNames(ros::NodeHandle& controller_nh, - const std::string& wheel_param, - std::vector& wheel_names); + bool setWheelParamsFromUrdf(ros::NodeHandle& root_nh, + const std::string& wheel0_name, + const std::string& wheel1_name, + const std::string& wheel2_name, + const std::string& wheel3_name); /** - * \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 + * \brief Get the radius of a given wheel + * \param model urdf model used + * \param wheel_link link of the wheel from which to get the radius + * \param[out] wheels_radius radius of the wheel read from the urdf */ - bool setOdomParamsFromUrdf(ros::NodeHandle& root_nh, - const std::string& left_wheel_name, - const std::string& right_wheel_name); + bool getWheelRadius(const boost::shared_ptr model, const boost::shared_ptr& wheel_link, double& wheel_radius); /** * \brief Sets the odometry publishing fields diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h index 9fc9c080a..40a360d01 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h @@ -79,12 +79,14 @@ namespace mecanum_drive_controller /** * \brief Updates the odometry class with latest wheels position - * \param left_pos Left wheel position [rad] - * \param right_pos Right wheel position [rad] + * \param wheel0_vel Wheel velocity [rad] + * \param wheel1_vel Wheel velocity [rad] + * \param wheel2_vel Wheel velocity [rad] + * \param wheel3_vel Wheel velocity [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); + bool update(double wheel0_vel, double wheel1_vel, double wheel2_vel, double wheel3_vel, const ros::Time &time); /** * \brief Updates the odometry class with latest velocity command @@ -149,11 +151,11 @@ namespace mecanum_drive_controller } /** - * \brief Sets the wheel parameters: radius and separation - * \param wheel_separation Seperation between left and right wheels [m] - * \param wheel_radius Wheel radius [m] + * \brief Sets the wheels parameters: mecanum geometric param and radius + * \param wheels_k Wheels geometric param (used in mecanum wheels' ik) [m] + * \param wheels_radius Wheels radius [m] */ - void setWheelParams(double wheel_separation, double wheel_radius); + void setWheelsParams(double wheels_k, double wheels_radius); private: @@ -161,16 +163,10 @@ namespace mecanum_drive_controller typedef bacc::accumulator_set > 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 linearX, 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 linearX Linear velocity along X [m] (linear displacement, i.e. m/s * dt) computed by encoders + * \param linearY Linear velocity along Y [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 linearX, double linearY, double angular); @@ -188,13 +184,9 @@ namespace mecanum_drive_controller double linearY_; // [m/s] double angular_; // [rad/s] - /// Wheel kinematic parameters [m]: - double wheel_separation_; - double wheel_radius_; - - /// Previou wheel position/state [rad]: - double left_wheel_old_pos_; - double right_wheel_old_pos_; + /// Wheels kinematic parameters [m]: + double wheels_k_; + double wheels_radius_; /// Rolling mean accumulators for the linar and angular velocities: size_t velocity_rolling_window_size_; diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 83e8b40d6..756e4fb60 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -44,18 +44,7 @@ #include -static double euclideanOfVectors(const urdf::Vector3& vec1, const urdf::Vector3& vec2) -{ - return std::sqrt(std::pow(vec1.x-vec2.x,2) + - std::pow(vec1.y-vec2.y,2) + - std::pow(vec1.z-vec2.z,2)); -} - -/* - * \brief Check if the link is modeled as a cylinder or a sphere - * \param link Link - * \return true if the link is modeled as a Cylinder or Sphere; false otherwise - */ +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// static bool isCylinderOrSphere(const boost::shared_ptr& link) { if(!link) @@ -85,40 +74,16 @@ static bool isCylinderOrSphere(const boost::shared_ptr& link) 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 boost::shared_ptr& wheel_link, double& wheel_radius) +namespace mecanum_drive_controller { - if(!isCylinderOrSphere(wheel_link)) - { - ROS_ERROR_STREAM("Wheel link " << wheel_link->name << " is NOT modeled as a cylinder!"); - return false; - } - - if (wheel_link->collision->geometry->type == urdf::Geometry::CYLINDER) - wheel_radius = (static_cast(wheel_link->collision->geometry.get()))->radius; - else - wheel_radius = (static_cast(wheel_link->collision->geometry.get()))->radius; - - ROS_INFO_STREAM(" radius: " << wheel_radius); - - return true; -} - -namespace mecanum_drive_controller{ +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// MecanumDriveController::MecanumDriveController() : open_loop_(false) , command_struct_() - , wheel_separation_(0.0) , use_realigned_roller_joints_(false) - , wheel_radius_(0.0) - , wheel_separation_multiplier_(1.0) - , wheel_radius_multiplier_(1.0) + , wheels_k_(0.0) + , wheels_radius_(0.0) , cmd_vel_timeout_(0.5) , base_frame_id_("base_link") , enable_odom_tf_(true) @@ -126,6 +91,8 @@ namespace mecanum_drive_controller{ { } + + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool MecanumDriveController::init(hardware_interface::VelocityJointInterface* hw, ros::NodeHandle& root_nh, ros::NodeHandle &controller_nh) @@ -134,31 +101,26 @@ namespace mecanum_drive_controller{ std::size_t id = complete_ns.find_last_of("/"); name_ = complete_ns.substr(id + 1); - // Get joint names from the parameter server - controller_nh.param("use_realigned_roller_joints", use_realigned_roller_joints_, false); + // Option use_realigned_roller_joints + controller_nh.param("use_realigned_roller_joints", use_realigned_roller_joints_, use_realigned_roller_joints_); ROS_INFO_STREAM_NAMED(name_, "Use the roller's radius rather than the wheel's: " << use_realigned_roller_joints_ << "."); - std::vector left_wheel_names, right_wheel_names; - if (!getWheelNames(controller_nh, "left_wheel", left_wheel_names) or - !getWheelNames(controller_nh, "right_wheel", right_wheel_names)) - { - return false; - } + // Get joint names from the parameter server + std::string wheel0_name; + controller_nh.param("front_left_wheel_joint", wheel0_name, wheel0_name); + ROS_INFO_STREAM_NAMED(name_, "Front left wheel joint (wheel0) is : " << wheel0_name); - 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(); + std::string wheel1_name; + controller_nh.param("back_left_wheel_joint", wheel1_name, wheel1_name); + ROS_INFO_STREAM_NAMED(name_, "Back left wheel joint (wheel1) is : " << wheel1_name); - left_wheel_joints_.resize(wheel_joints_size_); - right_wheel_joints_.resize(wheel_joints_size_); - } + std::string wheel2_name; + controller_nh.param("back_right_wheel_joint", wheel2_name, wheel2_name); + ROS_INFO_STREAM_NAMED(name_, "Back right wheel joint (wheel2) is : " << wheel2_name); + + std::string wheel3_name; + controller_nh.param("front_right_wheel_joint", wheel3_name, wheel3_name); + ROS_INFO_STREAM_NAMED(name_, "Front right wheel joint (wheel3) is : " << wheel3_name); // Odometry related: double publish_rate; @@ -169,14 +131,6 @@ namespace mecanum_drive_controller{ 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_ << "."); - - controller_nh.param("wheel_radius_multiplier", wheel_radius_multiplier_, wheel_radius_multiplier_); - ROS_INFO_STREAM_NAMED(name_, "Wheel radius will be multiplied by " - << wheel_radius_multiplier_ << "."); - // 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 " @@ -210,26 +164,25 @@ namespace mecanum_drive_controller{ 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 ); - if (!setOdomParamsFromUrdf(root_nh, left_wheel_names[0], right_wheel_names[0])) + // Get the joint objects to use in the realtime loop + wheel0_jointHandle_ = hw->getHandle(wheel0_name); // throws on failure + wheel1_jointHandle_ = hw->getHandle(wheel1_name); // throws on failure + wheel2_jointHandle_ = hw->getHandle(wheel2_name); // throws on failure + wheel3_jointHandle_ = hw->getHandle(wheel3_name); // throws on failure + + // Pass params through and setup publishers and subscribers + if (!setWheelParamsFromUrdf(root_nh, wheel0_name, wheel1_name, wheel2_name, wheel3_name)) return false; setOdomPubFields(root_nh, controller_nh); - // Get the joint object to use in the realtime loop - for (int 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, &MecanumDriveController::cmdVelCallback, this); return true; } + + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void MecanumDriveController::update(const ros::Time& time, const ros::Duration& period) { // COMPUTE AND PUBLISH ODOMETRY @@ -239,23 +192,16 @@ namespace mecanum_drive_controller{ } 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; + double wheel0_vel = wheel0_jointHandle_.getVelocity(); + double wheel1_vel = wheel1_jointHandle_.getVelocity(); + double wheel2_vel = wheel2_jointHandle_.getVelocity(); + double wheel3_vel = wheel3_jointHandle_.getVelocity(); - left_pos += lp; - right_pos += rp; - } - left_pos /= wheel_joints_size_; - right_pos /= wheel_joints_size_; + if (std::isnan(wheel0_vel) || std::isnan(wheel1_vel) || std::isnan(wheel2_vel) || std::isnan(wheel3_vel)) + return; - // Estimate linear and angular velocity using joint information - odometry_.update(left_pos, right_pos, time); + // Estimate twist (using joint information) and integrate + odometry_.update(wheel0_vel, wheel1_vel, wheel2_vel, wheel3_vel, time); } // Publish odometry message @@ -292,7 +238,7 @@ namespace mecanum_drive_controller{ } // MOVE ROBOT - // Retreive current velocity command and time step: + // Retrieve current velocity command and time step: Commands curr_cmd = *(command_.readFromRT()); const double dt = (time - curr_cmd.stamp).toSec(); @@ -311,22 +257,20 @@ namespace mecanum_drive_controller{ limiter_ang_.limit(curr_cmd.ang, last_cmd_.ang, cmd_dt); last_cmd_ = curr_cmd; - // Apply multipliers: - const double ws = wheel_separation_multiplier_ * wheel_separation_; - const double wr = wheel_radius_multiplier_ * wheel_radius_; - // Compute wheels velocities: - const double vel_left = (curr_cmd.linX - curr_cmd.ang * ws / 2.0)/wr; - const double vel_right = (curr_cmd.linX + curr_cmd.ang * ws / 2.0)/wr; + const double w0_vel = 1.0 / wheels_radius_ * (curr_cmd.linX - curr_cmd.linY - wheels_k_ * curr_cmd.ang); + const double w1_vel = 1.0 / wheels_radius_ * (curr_cmd.linX + curr_cmd.linY - wheels_k_ * curr_cmd.ang); + const double w2_vel = 1.0 / wheels_radius_ * (curr_cmd.linX - curr_cmd.linY + wheels_k_ * curr_cmd.ang); + const double w3_vel = 1.0 / wheels_radius_ * (curr_cmd.linX + curr_cmd.linY + wheels_k_ * curr_cmd.ang); // 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); - } + wheel0_jointHandle_.setCommand(w0_vel); + wheel1_jointHandle_.setCommand(w1_vel); + wheel2_jointHandle_.setCommand(w2_vel); + wheel3_jointHandle_.setCommand(w3_vel); } + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void MecanumDriveController::starting(const ros::Time& time) { brake(); @@ -337,28 +281,29 @@ namespace mecanum_drive_controller{ odometry_.init(time); } + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void MecanumDriveController::stopping(const ros::Time& time) { brake(); } + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void MecanumDriveController::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); - } + wheel0_jointHandle_.setCommand(0.0); + wheel1_jointHandle_.setCommand(0.0); + wheel2_jointHandle_.setCommand(0.0); + wheel3_jointHandle_.setCommand(0.0); } + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void MecanumDriveController::cmdVelCallback(const geometry_msgs::Twist& command) { if(isRunning()) { command_struct_.ang = command.angular.z; - command_struct_.linX = command.linear.x; - command_struct_.linY = command.linear.y; + command_struct_.linX = command.linear.x; + command_struct_.linY = command.linear.y; command_struct_.stamp = ros::Time::now(); command_.writeFromNonRT (command_struct_); ROS_DEBUG_STREAM_NAMED(name_, @@ -374,62 +319,12 @@ namespace mecanum_drive_controller{ } } - bool MecanumDriveController::getWheelNames(ros::NodeHandle& controller_nh, - const std::string& wheel_param, - std::vector& 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(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 MecanumDriveController::setOdomParamsFromUrdf(ros::NodeHandle& root_nh, - const std::string& left_wheel_name, - const std::string& right_wheel_name) + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + bool MecanumDriveController::setWheelParamsFromUrdf(ros::NodeHandle& root_nh, + const std::string& wheel0_name, + const std::string& wheel1_name, + const std::string& wheel2_name, + const std::string& wheel3_name) { // Parse robot description const std::string model_param_name = "robot_description"; @@ -443,68 +338,131 @@ namespace mecanum_drive_controller{ boost::shared_ptr model(urdf::parseURDF(robot_model_str)); - // Get wheel separation - boost::shared_ptr left_wheel_joint(model->getJoint(left_wheel_name)); - if(!left_wheel_joint) + // Get wheels position and compute parameter k_ (used in mecanum wheels IK). + boost::shared_ptr wheel0_urdfJoint(model->getJoint(wheel0_name)); + if(!wheel0_urdfJoint) + { + ROS_ERROR_STREAM_NAMED(name_, wheel0_name + << " couldn't be retrieved from model description"); + return false; + } + boost::shared_ptr wheel1_urdfJoint(model->getJoint(wheel1_name)); + if(!wheel1_urdfJoint) { - ROS_ERROR_STREAM_NAMED(name_, left_wheel_name + ROS_ERROR_STREAM_NAMED(name_, wheel1_name << " couldn't be retrieved from model description"); return false; } - boost::shared_ptr right_wheel_joint(model->getJoint(right_wheel_name)); - if(!right_wheel_joint) + boost::shared_ptr wheel2_urdfJoint(model->getJoint(wheel2_name)); + if(!wheel2_urdfJoint) { - ROS_ERROR_STREAM_NAMED(name_, right_wheel_name + ROS_ERROR_STREAM_NAMED(name_, wheel2_name << " couldn't be retrieved from model description"); return false; } + boost::shared_ptr wheel3_urdfJoint(model->getJoint(wheel3_name)); + if(!wheel3_urdfJoint) + { + ROS_ERROR_STREAM_NAMED(name_, wheel3_name + << " couldn't be retrieved from model description"); + return false; + } + + ROS_INFO_STREAM("wheel0 to origin: " << wheel0_urdfJoint->parent_to_joint_origin_transform.position.x << "," + << wheel0_urdfJoint->parent_to_joint_origin_transform.position.y << ", " + << wheel0_urdfJoint->parent_to_joint_origin_transform.position.z); + ROS_INFO_STREAM("wheel1 to origin: " << wheel1_urdfJoint->parent_to_joint_origin_transform.position.x << "," + << wheel1_urdfJoint->parent_to_joint_origin_transform.position.y << ", " + << wheel1_urdfJoint->parent_to_joint_origin_transform.position.z); + ROS_INFO_STREAM("wheel2 to origin: " << wheel2_urdfJoint->parent_to_joint_origin_transform.position.x << "," + << wheel2_urdfJoint->parent_to_joint_origin_transform.position.y << ", " + << wheel2_urdfJoint->parent_to_joint_origin_transform.position.z); + ROS_INFO_STREAM("wheel3 to origin: " << wheel3_urdfJoint->parent_to_joint_origin_transform.position.x << "," + << wheel3_urdfJoint->parent_to_joint_origin_transform.position.y << ", " + << wheel3_urdfJoint->parent_to_joint_origin_transform.position.z); + + double wheel0_x = wheel0_urdfJoint->parent_to_joint_origin_transform.position.x; + double wheel0_y = wheel0_urdfJoint->parent_to_joint_origin_transform.position.y; + double wheel1_x = wheel1_urdfJoint->parent_to_joint_origin_transform.position.x; + double wheel1_y = wheel1_urdfJoint->parent_to_joint_origin_transform.position.y; + double wheel2_x = wheel2_urdfJoint->parent_to_joint_origin_transform.position.x; + double wheel2_y = wheel2_urdfJoint->parent_to_joint_origin_transform.position.y; + double wheel3_x = wheel3_urdfJoint->parent_to_joint_origin_transform.position.x; + double wheel3_y = wheel3_urdfJoint->parent_to_joint_origin_transform.position.y; + + wheels_k_ = (-(-wheel0_x - wheel0_y) - (wheel1_x - wheel1_y) + (-wheel2_x - wheel2_y) + (wheel3_x - wheel3_y)) / 4.0; + + // Get wheels radius + double wheel0_radius = 0.0; + double wheel1_radius = 0.0; + double wheel2_radius = 0.0; + double wheel3_radius = 0.0; + + if (!getWheelRadius(model, model->getLink(wheel0_urdfJoint->child_link_name), wheel0_radius) || + !getWheelRadius(model, model->getLink(wheel1_urdfJoint->child_link_name), wheel1_radius) || + !getWheelRadius(model, model->getLink(wheel2_urdfJoint->child_link_name), wheel2_radius) || + !getWheelRadius(model, model->getLink(wheel3_urdfJoint->child_link_name), wheel3_radius)) + { + ROS_ERROR_STREAM_NAMED(name_, "Couldn't retrieve wheels' radius"); + return false; + } + + if (abs(wheel0_radius - wheel1_radius) > 1e-3 || + abs(wheel0_radius - wheel2_radius) > 1e-3 || + abs(wheel0_radius - wheel3_radius) > 1e-3) + { + ROS_ERROR_STREAM_NAMED(name_, "Wheels radius are not egual"); + return false; + } + + wheels_radius_ = wheel0_radius; + + ROS_INFO_STREAM("Wheel radius: " << wheels_radius_); - 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); + // Set wheel params for the odometry computation + odometry_.setWheelsParams(wheels_k_, wheels_radius_); - wheel_separation_ = euclideanOfVectors(left_wheel_joint->parent_to_joint_origin_transform.position, - right_wheel_joint->parent_to_joint_origin_transform.position); + return true; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + bool MecanumDriveController::getWheelRadius(const boost::shared_ptr model, const boost::shared_ptr& wheel_link, double& wheel_radius) + { + boost::shared_ptr radius_link = wheel_link; - boost::shared_ptr radius_link = model->getLink(left_wheel_joint->child_link_name); if (use_realigned_roller_joints_) { // This mode is used when the mecanum wheels are simulated and we use realigned rollers to mimic mecanum wheels. const boost::shared_ptr& roller_joint = radius_link->child_joints[0]; if(!roller_joint) { - ROS_ERROR_STREAM_NAMED(name_, "No roller joint could be retrieved for wheel : " << left_wheel_joint->child_link_name << ". Are you sure mecanum wheels are simulated using realigned rollers?"); + ROS_ERROR_STREAM_NAMED(name_, "No roller joint could be retrieved for wheel : " << wheel_link->name << ". Are you sure mecanum wheels are simulated using realigned rollers?"); return false; } radius_link = model->getLink(roller_joint->child_link_name); if(!radius_link) { - ROS_ERROR_STREAM_NAMED(name_, "No roller link could be retrieved for wheel : " << left_wheel_joint->child_link_name << ". Are you sure mecanum wheels are simulated using realigned rollers?"); + ROS_ERROR_STREAM_NAMED(name_, "No roller link could be retrieved for wheel : " << wheel_link->name << ". Are you sure mecanum wheels are simulated using realigned rollers?"); return false; } } - // Get wheel radius - if(!getWheelRadius(radius_link, wheel_radius_)) + if(!isCylinderOrSphere(radius_link)) { - ROS_ERROR_STREAM_NAMED(name_, "Couldn't retrieve " << left_wheel_name << " wheel radius"); + ROS_ERROR_STREAM("Wheel link " << radius_link->name << " is NOT modeled as a cylinder!"); return false; } - // Set wheel params for the odometry computation - const double ws = wheel_separation_multiplier_ * wheel_separation_; - const double wr = wheel_radius_multiplier_ * wheel_radius_; - odometry_.setWheelParams(ws, wr); - ROS_INFO_STREAM_NAMED(name_, - "Odometry params : wheel separation " << ws - << ", wheel radius " << wr); + if (radius_link->collision->geometry->type == urdf::Geometry::CYLINDER) + wheel_radius = (static_cast(radius_link->collision->geometry.get()))->radius; + else + wheel_radius = (static_cast(radius_link->collision->geometry.get()))->radius; + return true; } + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void MecanumDriveController::setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) { // Get and check params for covariances diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index 5e778dd7a..ce9d31689 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -47,6 +47,7 @@ namespace mecanum_drive_controller { namespace bacc = boost::accumulators; + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// Odometry::Odometry(size_t velocity_rolling_window_size) : timestamp_(0.0) , x_(0.0) @@ -55,10 +56,8 @@ namespace mecanum_drive_controller , linearX_(0.0) , linearY_(0.0) , angular_(0.0) - , wheel_separation_(0.0) - , wheel_radius_(0.0) - , left_wheel_old_pos_(0.0) - , right_wheel_old_pos_(0.0) + , wheels_k_(0.0) + , wheels_radius_(0.0) , velocity_rolling_window_size_(velocity_rolling_window_size) , linearX_acc_(RollingWindow::window_size = velocity_rolling_window_size) , linearY_acc_(RollingWindow::window_size = velocity_rolling_window_size) @@ -67,6 +66,7 @@ namespace mecanum_drive_controller { } + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void Odometry::init(const ros::Time& time) { // Reset accumulators: @@ -78,35 +78,29 @@ namespace mecanum_drive_controller timestamp_ = time; } - bool Odometry::update(double left_pos, double right_pos, const ros::Time &time) + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + bool Odometry::update(double wheel0_vel, double wheel1_vel, double wheel2_vel, double wheel3_vel, const ros::Time &time) { - /// Get current wheel joint positions: - const double left_wheel_cur_pos = left_pos * wheel_radius_; - const double right_wheel_cur_pos = right_pos * wheel_radius_; + /// 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 - /// 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_; + timestamp_ = time; - /// Update old position with current: - left_wheel_old_pos_ = left_wheel_cur_pos; - right_wheel_old_pos_ = right_wheel_cur_pos; + /// Compute forward kinematics (i.e. compute mobile robot's twist out of its wheels velocities): + /// Note: we use the IK of the mecanum wheels which we invert using a pseudo-inverse. + double linearX = 0.25 * wheels_radius_ * ( wheel0_vel + wheel1_vel + wheel2_vel + wheel3_vel); + double linearY = 0.25 * wheels_radius_ * (-wheel0_vel + wheel1_vel - wheel2_vel + wheel3_vel); + double angular = 0.25 * wheels_radius_ / wheels_k_ * (-wheel0_vel - wheel1_vel + wheel2_vel + wheel3_vel); - /// Compute linear and angular diff: - const double linearX = (right_wheel_est_vel + left_wheel_est_vel) * 0.5 ; - const double linearY = 0.0; - const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_; + linearX *= dt; + linearY *= dt; + angular *= dt; /// Integrate odometry: integrate_fun_(linearX, linearY, 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: linearX_acc_(linearX/dt); linearY_acc_(linearY/dt); @@ -119,6 +113,7 @@ namespace mecanum_drive_controller return true; } + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void Odometry::updateOpenLoop(double linearX, double linearY, double angular, const ros::Time &time) { /// Save last linear and angular velocity: @@ -132,40 +127,20 @@ namespace mecanum_drive_controller integrate_fun_(linearX * dt, linearY * dt, angular * dt); } - void Odometry::setWheelParams(double wheel_separation, double wheel_radius) + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + void Odometry::setWheelsParams(double wheels_k, double wheels_radius) { - wheel_separation_ = wheel_separation; - wheel_radius_ = wheel_radius; - } + wheels_k_ = wheels_k; - void Odometry::integrateRungeKutta2(double linearX, double angular) - { - const double direction = heading_ + angular * 0.5; - - /// Runge-Kutta 2nd order integration: - x_ += linearX * cos(direction); - y_ += linearX * sin(direction); - heading_ += angular; + wheels_radius_ = wheels_radius; } - /** - * \brief Other possible integration method provided by the class - * \param linear - * \param angular - */ + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void Odometry::integrateExact(double linearX, double linearY, double angular) { - if(fabs(angular) < 10e-3) - integrateRungeKutta2(linearX, angular); - else - { - /// Exact integration (should solve problems when angular is zero): - const double heading_old = heading_; - const double r = linearX/angular; - heading_ += angular; - x_ += r * (sin(heading_) - sin(heading_old)); - y_ += -r * (cos(heading_) - cos(heading_old)); - } + heading_ += angular; + x_ += linearX; + y_ += linearY; } } // namespace mecanum_drive_controller From 71a12a8fa7ea204aa47f68fb19711fcfbf6d3f79 Mon Sep 17 00:00:00 2001 From: Antoine Rennuit Date: Wed, 29 Oct 2014 16:48:02 +0100 Subject: [PATCH 09/21] Namespace cleaning (should not change behavior). --- .../mecanum_drive_controller.h | 282 +++---- .../mecanum_drive_controller/odometry.h | 288 +++---- .../src/mecanum_drive_controller.cpp | 770 +++++++++--------- mecanum_drive_controller/src/odometry.cpp | 195 ++--- 4 files changed, 770 insertions(+), 765 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h index 28f9343b9..000105d32 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h @@ -49,151 +49,153 @@ #include #include -namespace mecanum_drive_controller{ +namespace mecanum_drive_controller +{ + +/** + * This class makes some assumptions on the model of the robot: + * - the rotation axes of wheels are collinear + * - the wheels are identical in radius + * Additional assumptions to not duplicate information readily available in the URDF: + * - the wheels have the same parent frame + * - a wheel collision geometry is a cylinder in the urdf + * - a wheel joint frame center's vertical projection on the floor must lie within the contact patch + */ +class MecanumDriveController + : public controller_interface::Controller +{ +public: + MecanumDriveController(); + + /** + * \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); /** - * This class makes some assumptions on the model of the robot: - * - the rotation axes of wheels are collinear - * - the wheels are identical in radius - * Additional assumptions to not duplicate information readily available in the URDF: - * - the wheels have the same parent frame - * - a wheel collision geometry is a cylinder in the urdf - * - a wheel joint frame center's vertical projection on the floor must lie within the contact patch + * \brief Stops controller + * \param time Current time */ - class MecanumDriveController - : public controller_interface::Controller + void stopping(const ros::Time& time); + +private: + std::string name_; + + /// Odometry related: + ros::Duration publish_period_; + ros::Time last_state_publish_time_; + bool open_loop_; + + /// Hardware handles: + hardware_interface::JointHandle wheel0_jointHandle_; + hardware_interface::JointHandle wheel1_jointHandle_; + hardware_interface::JointHandle wheel2_jointHandle_; + hardware_interface::JointHandle wheel3_jointHandle_; + + /// Velocity command related: + struct Commands { - public: - MecanumDriveController(); - - /** - * \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: - hardware_interface::JointHandle wheel0_jointHandle_; - hardware_interface::JointHandle wheel1_jointHandle_; - hardware_interface::JointHandle wheel2_jointHandle_; - hardware_interface::JointHandle wheel3_jointHandle_; - - /// Velocity command related: - struct Commands - { - double linX; - double linY; - double ang; - ros::Time stamp; - - Commands() : linX(0.0), linY(0.0), ang(0.0), stamp(0.0) {} - }; - realtime_tools::RealtimeBuffer command_; - Commands command_struct_; - ros::Subscriber sub_command_; - - /// Odometry related: - boost::shared_ptr > odom_pub_; - boost::shared_ptr > tf_odom_pub_; - Odometry odometry_; - geometry_msgs::TransformStamped odom_frame_; - - /// Wheel radius (assuming it's the same for the left and right wheels): - bool use_realigned_roller_joints_; - double wheels_k_; // wheels geometric param used in mecanum wheels' ik - double wheels_radius_; - - /// Timeout to consider cmd_vel commands old: - double cmd_vel_timeout_; - - /// Frame to use for the robot base: - std::string base_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 last_cmd_; - SpeedLimiter limiter_linX_; - SpeedLimiter limiter_linY_; - SpeedLimiter limiter_ang_; - - private: - /** - * \brief Brakes the wheels, i.e. sets the velocity to 0 - */ - void brake(); - - /** - * \brief Velocity command callback - * \param command Velocity command message (twist) - */ - void cmdVelCallback(const geometry_msgs::Twist& command); - - /** - * \brief Sets odometry parameters from the URDF, i.e. the wheel radius and separation - * \param root_nh Root node handle - * \param wheel0_name Name of wheel0 joint - * \param wheel1_name Name of wheel1 joint - * \param wheel2_name Name of wheel2 joint - * \param wheel3_name Name of wheel3 joint - */ - bool setWheelParamsFromUrdf(ros::NodeHandle& root_nh, - const std::string& wheel0_name, - const std::string& wheel1_name, - const std::string& wheel2_name, - const std::string& wheel3_name); - - /** - * \brief Get the radius of a given wheel - * \param model urdf model used - * \param wheel_link link of the wheel from which to get the radius - * \param[out] wheels_radius radius of the wheel read from the urdf - */ - bool getWheelRadius(const boost::shared_ptr model, const boost::shared_ptr& wheel_link, double& 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); + double linX; + double linY; + double ang; + ros::Time stamp; + Commands() : linX(0.0), linY(0.0), ang(0.0), stamp(0.0) {} }; + realtime_tools::RealtimeBuffer command_; + Commands command_struct_; + ros::Subscriber sub_command_; + + /// Odometry related: + boost::shared_ptr > odom_pub_; + boost::shared_ptr > tf_odom_pub_; + Odometry odometry_; + geometry_msgs::TransformStamped odom_frame_; + + /// Wheel radius (assuming it's the same for the left and right wheels): + bool use_realigned_roller_joints_; + double wheels_k_; // wheels geometric param used in mecanum wheels' ik + double wheels_radius_; + + /// Timeout to consider cmd_vel commands old: + double cmd_vel_timeout_; + + /// Frame to use for the robot base: + std::string base_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 last_cmd_; + SpeedLimiter limiter_linX_; + SpeedLimiter limiter_linY_; + SpeedLimiter limiter_ang_; + +private: + /** + * \brief Brakes the wheels, i.e. sets the velocity to 0 + */ + void brake(); + + /** + * \brief Velocity command callback + * \param command Velocity command message (twist) + */ + void cmdVelCallback(const geometry_msgs::Twist& command); + + /** + * \brief Sets odometry parameters from the URDF, i.e. the wheel radius and separation + * \param root_nh Root node handle + * \param wheel0_name Name of wheel0 joint + * \param wheel1_name Name of wheel1 joint + * \param wheel2_name Name of wheel2 joint + * \param wheel3_name Name of wheel3 joint + */ + bool setWheelParamsFromUrdf(ros::NodeHandle& root_nh, + const std::string& wheel0_name, + const std::string& wheel1_name, + const std::string& wheel2_name, + const std::string& wheel3_name); + + /** + * \brief Get the radius of a given wheel + * \param model urdf model used + * \param wheel_link link of the wheel from which to get the radius + * \param[out] wheels_radius radius of the wheel read from the urdf + */ + bool getWheelRadius(const boost::shared_ptr model, const boost::shared_ptr& wheel_link, double& wheel_radius); + + /** + * \brief Sets the odometry publishing fields + * \param root_nh Root node handle + * \param controller_nh Node handle inside the controller namespace + */ + void setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh); + +}; + +PLUGINLIB_EXPORT_CLASS(mecanum_drive_controller::MecanumDriveController, controller_interface::ControllerBase) - PLUGINLIB_EXPORT_CLASS(mecanum_drive_controller::MecanumDriveController, controller_interface::ControllerBase) } // namespace mecanum_drive_controller diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h index 40a360d01..12d746580 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h @@ -50,153 +50,155 @@ namespace mecanum_drive_controller { - namespace bacc = boost::accumulators; + +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 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 wheel0_vel Wheel velocity [rad] + * \param wheel1_vel Wheel velocity [rad] + * \param wheel2_vel Wheel velocity [rad] + * \param wheel3_vel Wheel velocity [rad] + * \param time Current time + * \return true if the odometry is actually updated + */ + bool update(double wheel0_vel, double wheel1_vel, double wheel2_vel, double wheel3_vel, const ros::Time &time); + + /** + * \brief Updates the odometry class with latest velocity command + * \param linearX Linear velocity [m/s] + * \param angular Angular velocity [rad/s] + * \param time Current time + */ + void updateOpenLoop(double linearX, double linearY, 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 linearX velocity getter + * \return linearX velocity [m/s] + */ + double getLinearX() const + { + return linearX_; + } /** - * \brief The Odometry class handles odometry readings - * (2D pose and velocity with related timestamp) + * \brief linearY velocity getter + * \return linearY velocity [m/s] */ - class Odometry + double getLinearY() const { - public: - - /// Integration function, used to integrate the odometry: - typedef boost::function 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 wheel0_vel Wheel velocity [rad] - * \param wheel1_vel Wheel velocity [rad] - * \param wheel2_vel Wheel velocity [rad] - * \param wheel3_vel Wheel velocity [rad] - * \param time Current time - * \return true if the odometry is actually updated - */ - bool update(double wheel0_vel, double wheel1_vel, double wheel2_vel, double wheel3_vel, const ros::Time &time); - - /** - * \brief Updates the odometry class with latest velocity command - * \param linearX Linear velocity [m/s] - * \param angular Angular velocity [rad/s] - * \param time Current time - */ - void updateOpenLoop(double linearX, double linearY, 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 linearX velocity getter - * \return linearX velocity [m/s] - */ - double getLinearX() const - { - return linearX_; - } - - /** - * \brief linearY velocity getter - * \return linearY velocity [m/s] - */ - double getLinearY() const - { - return linearY_; - } - - /** - * \brief angular velocity getter - * \return angular velocity [rad/s] - */ - double getAngular() const - { - return angular_; - } - - /** - * \brief Sets the wheels parameters: mecanum geometric param and radius - * \param wheels_k Wheels geometric param (used in mecanum wheels' ik) [m] - * \param wheels_radius Wheels radius [m] - */ - void setWheelsParams(double wheels_k, double wheels_radius); - - private: - - /// Rolling mean accumulator and window: - typedef bacc::accumulator_set > RollingMeanAcc; - typedef bacc::tag::rolling_window RollingWindow; - - /** - * \brief Integrates the velocities (linear and angular) using exact method - * \param linearX Linear velocity along X [m] (linear displacement, i.e. m/s * dt) computed by encoders - * \param linearY Linear velocity along Y [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 linearX, double linearY, double angular); - - /// Current timestamp: - ros::Time timestamp_; - - /// Current pose: - double x_; // [m] - double y_; // [m] - double heading_; // [rad] - - /// Current velocity: - double linearX_; // [m/s] - double linearY_; // [m/s] - double angular_; // [rad/s] - - /// Wheels kinematic parameters [m]: - double wheels_k_; - double wheels_radius_; - - /// Rolling mean accumulators for the linar and angular velocities: - size_t velocity_rolling_window_size_; - RollingMeanAcc linearX_acc_; - RollingMeanAcc linearY_acc_; - RollingMeanAcc angular_acc_; - - /// Integration funcion, used to integrate the odometry: - IntegrationFunction integrate_fun_; - }; + return linearY_; + } + + /** + * \brief angular velocity getter + * \return angular velocity [rad/s] + */ + double getAngular() const + { + return angular_; + } + + /** + * \brief Sets the wheels parameters: mecanum geometric param and radius + * \param wheels_k Wheels geometric param (used in mecanum wheels' ik) [m] + * \param wheels_radius Wheels radius [m] + */ + void setWheelsParams(double wheels_k, double wheels_radius); + +private: + + /// Rolling mean accumulator and window: + typedef bacc::accumulator_set > RollingMeanAcc; + typedef bacc::tag::rolling_window RollingWindow; + + /** + * \brief Integrates the velocities (linear and angular) using exact method + * \param linearX Linear velocity along X [m] (linear displacement, i.e. m/s * dt) computed by encoders + * \param linearY Linear velocity along Y [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 linearX, double linearY, double angular); + + /// Current timestamp: + ros::Time timestamp_; + + /// Current pose: + double x_; // [m] + double y_; // [m] + double heading_; // [rad] + + /// Current velocity: + double linearX_; // [m/s] + double linearY_; // [m/s] + double angular_; // [rad/s] + + /// Wheels kinematic parameters [m]: + double wheels_k_; + double wheels_radius_; + + /// Rolling mean accumulators for the linar and angular velocities: + size_t velocity_rolling_window_size_; + RollingMeanAcc linearX_acc_; + RollingMeanAcc linearY_acc_; + RollingMeanAcc angular_acc_; + + /// Integration funcion, used to integrate the odometry: + IntegrationFunction integrate_fun_; +}; + } // namespace mecanum_drive_controller #endif /* ODOMETRY_H_ */ diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 756e4fb60..36e501d01 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -78,436 +78,436 @@ namespace mecanum_drive_controller { //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - MecanumDriveController::MecanumDriveController() - : open_loop_(false) - , command_struct_() - , use_realigned_roller_joints_(false) - , wheels_k_(0.0) - , wheels_radius_(0.0) - , cmd_vel_timeout_(0.5) - , base_frame_id_("base_link") - , enable_odom_tf_(true) - , wheel_joints_size_(0) - { - } +MecanumDriveController::MecanumDriveController() + : open_loop_(false) + , command_struct_() + , use_realigned_roller_joints_(false) + , wheels_k_(0.0) + , wheels_radius_(0.0) + , cmd_vel_timeout_(0.5) + , base_frame_id_("base_link") + , enable_odom_tf_(true) + , wheel_joints_size_(0) +{ +} + + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +bool MecanumDriveController::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); + + // Option use_realigned_roller_joints + controller_nh.param("use_realigned_roller_joints", use_realigned_roller_joints_, use_realigned_roller_joints_); + ROS_INFO_STREAM_NAMED(name_, "Use the roller's radius rather than the wheel's: " << use_realigned_roller_joints_ << "."); + + // Get joint names from the parameter server + std::string wheel0_name; + controller_nh.param("front_left_wheel_joint", wheel0_name, wheel0_name); + ROS_INFO_STREAM_NAMED(name_, "Front left wheel joint (wheel0) is : " << wheel0_name); + + std::string wheel1_name; + controller_nh.param("back_left_wheel_joint", wheel1_name, wheel1_name); + ROS_INFO_STREAM_NAMED(name_, "Back left wheel joint (wheel1) is : " << wheel1_name); + + std::string wheel2_name; + controller_nh.param("back_right_wheel_joint", wheel2_name, wheel2_name); + ROS_INFO_STREAM_NAMED(name_, "Back right wheel joint (wheel2) is : " << wheel2_name); + + std::string wheel3_name; + controller_nh.param("front_right_wheel_joint", wheel3_name, wheel3_name); + ROS_INFO_STREAM_NAMED(name_, "Front right wheel joint (wheel3) is : " << wheel3_name); + + // Odometry related: + double publish_rate; + controller_nh.param("publish_rate", publish_rate, 50.0); + ROS_INFO_STREAM_NAMED(name_, "Controller state will be published at " + << publish_rate << "Hz."); + publish_period_ = ros::Duration(1.0 / publish_rate); + + controller_nh.param("open_loop", open_loop_, open_loop_); + + // 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("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("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_linX_.has_velocity_limits , limiter_linX_.has_velocity_limits ); + controller_nh.param("linear/x/has_acceleration_limits", limiter_linX_.has_acceleration_limits, limiter_linX_.has_acceleration_limits); + controller_nh.param("linear/x/max_velocity" , limiter_linX_.max_velocity , limiter_linX_.max_velocity ); + controller_nh.param("linear/x/min_velocity" , limiter_linX_.min_velocity , -limiter_linX_.max_velocity ); + controller_nh.param("linear/x/max_acceleration" , limiter_linX_.max_acceleration , limiter_linX_.max_acceleration ); + controller_nh.param("linear/x/min_acceleration" , limiter_linX_.min_acceleration , -limiter_linX_.max_acceleration ); + + controller_nh.param("linear/y/has_velocity_limits" , limiter_linY_.has_velocity_limits , limiter_linY_.has_velocity_limits ); + controller_nh.param("linear/y/has_acceleration_limits", limiter_linY_.has_acceleration_limits, limiter_linY_.has_acceleration_limits); + controller_nh.param("linear/y/max_velocity" , limiter_linY_.max_velocity , limiter_linY_.max_velocity ); + controller_nh.param("linear/y/min_velocity" , limiter_linY_.min_velocity , -limiter_linY_.max_velocity ); + controller_nh.param("linear/y/max_acceleration" , limiter_linY_.max_acceleration , limiter_linY_.max_acceleration ); + controller_nh.param("linear/y/min_acceleration" , limiter_linY_.min_acceleration , -limiter_linY_.max_acceleration ); + + 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/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 ); + + // Get the joint objects to use in the realtime loop + wheel0_jointHandle_ = hw->getHandle(wheel0_name); // throws on failure + wheel1_jointHandle_ = hw->getHandle(wheel1_name); // throws on failure + wheel2_jointHandle_ = hw->getHandle(wheel2_name); // throws on failure + wheel3_jointHandle_ = hw->getHandle(wheel3_name); // throws on failure + + // Pass params through and setup publishers and subscribers + if (!setWheelParamsFromUrdf(root_nh, wheel0_name, wheel1_name, wheel2_name, wheel3_name)) + return false; + + setOdomPubFields(root_nh, controller_nh); + + sub_command_ = controller_nh.subscribe("cmd_vel", 1, &MecanumDriveController::cmdVelCallback, this); + + return true; +} - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - bool MecanumDriveController::init(hardware_interface::VelocityJointInterface* hw, - ros::NodeHandle& root_nh, - ros::NodeHandle &controller_nh) +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void MecanumDriveController::update(const ros::Time& time, const ros::Duration& period) +{ + // COMPUTE AND PUBLISH ODOMETRY + if (open_loop_) { - const std::string complete_ns = controller_nh.getNamespace(); - std::size_t id = complete_ns.find_last_of("/"); - name_ = complete_ns.substr(id + 1); - - // Option use_realigned_roller_joints - controller_nh.param("use_realigned_roller_joints", use_realigned_roller_joints_, use_realigned_roller_joints_); - ROS_INFO_STREAM_NAMED(name_, "Use the roller's radius rather than the wheel's: " << use_realigned_roller_joints_ << "."); - - // Get joint names from the parameter server - std::string wheel0_name; - controller_nh.param("front_left_wheel_joint", wheel0_name, wheel0_name); - ROS_INFO_STREAM_NAMED(name_, "Front left wheel joint (wheel0) is : " << wheel0_name); - - std::string wheel1_name; - controller_nh.param("back_left_wheel_joint", wheel1_name, wheel1_name); - ROS_INFO_STREAM_NAMED(name_, "Back left wheel joint (wheel1) is : " << wheel1_name); - - std::string wheel2_name; - controller_nh.param("back_right_wheel_joint", wheel2_name, wheel2_name); - ROS_INFO_STREAM_NAMED(name_, "Back right wheel joint (wheel2) is : " << wheel2_name); - - std::string wheel3_name; - controller_nh.param("front_right_wheel_joint", wheel3_name, wheel3_name); - ROS_INFO_STREAM_NAMED(name_, "Front right wheel joint (wheel3) is : " << wheel3_name); - - // Odometry related: - double publish_rate; - controller_nh.param("publish_rate", publish_rate, 50.0); - ROS_INFO_STREAM_NAMED(name_, "Controller state will be published at " - << publish_rate << "Hz."); - publish_period_ = ros::Duration(1.0 / publish_rate); - - controller_nh.param("open_loop", open_loop_, open_loop_); - - // 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("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("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_linX_.has_velocity_limits , limiter_linX_.has_velocity_limits ); - controller_nh.param("linear/x/has_acceleration_limits", limiter_linX_.has_acceleration_limits, limiter_linX_.has_acceleration_limits); - controller_nh.param("linear/x/max_velocity" , limiter_linX_.max_velocity , limiter_linX_.max_velocity ); - controller_nh.param("linear/x/min_velocity" , limiter_linX_.min_velocity , -limiter_linX_.max_velocity ); - controller_nh.param("linear/x/max_acceleration" , limiter_linX_.max_acceleration , limiter_linX_.max_acceleration ); - controller_nh.param("linear/x/min_acceleration" , limiter_linX_.min_acceleration , -limiter_linX_.max_acceleration ); - - controller_nh.param("linear/y/has_velocity_limits" , limiter_linY_.has_velocity_limits , limiter_linY_.has_velocity_limits ); - controller_nh.param("linear/y/has_acceleration_limits", limiter_linY_.has_acceleration_limits, limiter_linY_.has_acceleration_limits); - controller_nh.param("linear/y/max_velocity" , limiter_linY_.max_velocity , limiter_linY_.max_velocity ); - controller_nh.param("linear/y/min_velocity" , limiter_linY_.min_velocity , -limiter_linY_.max_velocity ); - controller_nh.param("linear/y/max_acceleration" , limiter_linY_.max_acceleration , limiter_linY_.max_acceleration ); - controller_nh.param("linear/y/min_acceleration" , limiter_linY_.min_acceleration , -limiter_linY_.max_acceleration ); - - 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/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 ); - - // Get the joint objects to use in the realtime loop - wheel0_jointHandle_ = hw->getHandle(wheel0_name); // throws on failure - wheel1_jointHandle_ = hw->getHandle(wheel1_name); // throws on failure - wheel2_jointHandle_ = hw->getHandle(wheel2_name); // throws on failure - wheel3_jointHandle_ = hw->getHandle(wheel3_name); // throws on failure - - // Pass params through and setup publishers and subscribers - if (!setWheelParamsFromUrdf(root_nh, wheel0_name, wheel1_name, wheel2_name, wheel3_name)) - return false; - - setOdomPubFields(root_nh, controller_nh); - - sub_command_ = controller_nh.subscribe("cmd_vel", 1, &MecanumDriveController::cmdVelCallback, this); - - return true; + odometry_.updateOpenLoop(last_cmd_.linX, last_cmd_.linY, last_cmd_.ang, time); } + else + { + double wheel0_vel = wheel0_jointHandle_.getVelocity(); + double wheel1_vel = wheel1_jointHandle_.getVelocity(); + double wheel2_vel = wheel2_jointHandle_.getVelocity(); + double wheel3_vel = wheel3_jointHandle_.getVelocity(); + if (std::isnan(wheel0_vel) || std::isnan(wheel1_vel) || std::isnan(wheel2_vel) || std::isnan(wheel3_vel)) + return; - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - void MecanumDriveController::update(const ros::Time& time, const ros::Duration& period) + // Estimate twist (using joint information) and integrate + odometry_.update(wheel0_vel, wheel1_vel, wheel2_vel, wheel3_vel, time); + } + + // Publish odometry message + if(last_state_publish_time_ + publish_period_ < time) { - // COMPUTE AND PUBLISH ODOMETRY - if (open_loop_) + 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()) { - odometry_.updateOpenLoop(last_cmd_.linX, last_cmd_.linY, last_cmd_.ang, time); + 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_.getLinearX(); + odom_pub_->msg_.twist.twist.linear.y = odometry_.getLinearY(); + odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular(); + odom_pub_->unlockAndPublish(); } - else + + // Publish tf /odom frame + if (enable_odom_tf_ && tf_odom_pub_->trylock()) { - double wheel0_vel = wheel0_jointHandle_.getVelocity(); - double wheel1_vel = wheel1_jointHandle_.getVelocity(); - double wheel2_vel = wheel2_jointHandle_.getVelocity(); - double wheel3_vel = wheel3_jointHandle_.getVelocity(); + 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(); + } + } - if (std::isnan(wheel0_vel) || std::isnan(wheel1_vel) || std::isnan(wheel2_vel) || std::isnan(wheel3_vel)) - return; + // MOVE ROBOT + // Retrieve current velocity command and time step: + Commands curr_cmd = *(command_.readFromRT()); + const double dt = (time - curr_cmd.stamp).toSec(); - // Estimate twist (using joint information) and integrate - odometry_.update(wheel0_vel, wheel1_vel, wheel2_vel, wheel3_vel, time); - } + // Brake if cmd_vel has timeout: + if (dt > cmd_vel_timeout_) + { + curr_cmd.linX = 0.0; + curr_cmd.linY = 0.0; + curr_cmd.ang = 0.0; + } - // 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())); + // Limit velocities and accelerations: + const double cmd_dt(period.toSec()); + limiter_linX_.limit(curr_cmd.linX, last_cmd_.linX, cmd_dt); + limiter_linY_.limit(curr_cmd.linY, last_cmd_.linY, cmd_dt); + limiter_ang_.limit(curr_cmd.ang, last_cmd_.ang, cmd_dt); + last_cmd_ = curr_cmd; + + // Compute wheels velocities: + const double w0_vel = 1.0 / wheels_radius_ * (curr_cmd.linX - curr_cmd.linY - wheels_k_ * curr_cmd.ang); + const double w1_vel = 1.0 / wheels_radius_ * (curr_cmd.linX + curr_cmd.linY - wheels_k_ * curr_cmd.ang); + const double w2_vel = 1.0 / wheels_radius_ * (curr_cmd.linX - curr_cmd.linY + wheels_k_ * curr_cmd.ang); + const double w3_vel = 1.0 / wheels_radius_ * (curr_cmd.linX + curr_cmd.linY + wheels_k_ * curr_cmd.ang); + + // Set wheels velocities: + wheel0_jointHandle_.setCommand(w0_vel); + wheel1_jointHandle_.setCommand(w1_vel); + wheel2_jointHandle_.setCommand(w2_vel); + wheel3_jointHandle_.setCommand(w3_vel); +} - // 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_.getLinearX(); - odom_pub_->msg_.twist.twist.linear.y = odometry_.getLinearY(); - odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular(); - odom_pub_->unlockAndPublish(); - } +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void MecanumDriveController::starting(const ros::Time& time) +{ + brake(); - // 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(); - } - } + // Register starting time used to keep fixed rate + last_state_publish_time_ = time; - // MOVE ROBOT - // Retrieve current velocity command and time step: - Commands curr_cmd = *(command_.readFromRT()); - const double dt = (time - curr_cmd.stamp).toSec(); + odometry_.init(time); +} - // Brake if cmd_vel has timeout: - if (dt > cmd_vel_timeout_) - { - curr_cmd.linX = 0.0; - curr_cmd.linY = 0.0; - curr_cmd.ang = 0.0; - } +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void MecanumDriveController::stopping(const ros::Time& time) +{ + brake(); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void MecanumDriveController::brake() +{ + wheel0_jointHandle_.setCommand(0.0); + wheel1_jointHandle_.setCommand(0.0); + wheel2_jointHandle_.setCommand(0.0); + wheel3_jointHandle_.setCommand(0.0); +} - // Limit velocities and accelerations: - const double cmd_dt(period.toSec()); - limiter_linX_.limit(curr_cmd.linX, last_cmd_.linX, cmd_dt); - limiter_linY_.limit(curr_cmd.linY, last_cmd_.linY, cmd_dt); - limiter_ang_.limit(curr_cmd.ang, last_cmd_.ang, cmd_dt); - last_cmd_ = curr_cmd; - - // Compute wheels velocities: - const double w0_vel = 1.0 / wheels_radius_ * (curr_cmd.linX - curr_cmd.linY - wheels_k_ * curr_cmd.ang); - const double w1_vel = 1.0 / wheels_radius_ * (curr_cmd.linX + curr_cmd.linY - wheels_k_ * curr_cmd.ang); - const double w2_vel = 1.0 / wheels_radius_ * (curr_cmd.linX - curr_cmd.linY + wheels_k_ * curr_cmd.ang); - const double w3_vel = 1.0 / wheels_radius_ * (curr_cmd.linX + curr_cmd.linY + wheels_k_ * curr_cmd.ang); - - // Set wheels velocities: - wheel0_jointHandle_.setCommand(w0_vel); - wheel1_jointHandle_.setCommand(w1_vel); - wheel2_jointHandle_.setCommand(w2_vel); - wheel3_jointHandle_.setCommand(w3_vel); +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void MecanumDriveController::cmdVelCallback(const geometry_msgs::Twist& command) +{ + if(isRunning()) + { + command_struct_.ang = command.angular.z; + command_struct_.linX = command.linear.x; + command_struct_.linY = command.linear.y; + 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_.linX << ", " + << "Lin: " << command_struct_.linY << ", " + << "Stamp: " << command_struct_.stamp); + } + else + { + ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running."); } +} - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - void MecanumDriveController::starting(const ros::Time& time) +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +bool MecanumDriveController::setWheelParamsFromUrdf(ros::NodeHandle& root_nh, + const std::string& wheel0_name, + const std::string& wheel1_name, + const std::string& wheel2_name, + const std::string& wheel3_name) +{ + // 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)) { - brake(); + ROS_ERROR_NAMED(name_, "Robot descripion couldn't be retrieved from param server."); + return false; + } - // Register starting time used to keep fixed rate - last_state_publish_time_ = time; + boost::shared_ptr model(urdf::parseURDF(robot_model_str)); - odometry_.init(time); + // Get wheels position and compute parameter k_ (used in mecanum wheels IK). + boost::shared_ptr wheel0_urdfJoint(model->getJoint(wheel0_name)); + if(!wheel0_urdfJoint) + { + ROS_ERROR_STREAM_NAMED(name_, wheel0_name + << " couldn't be retrieved from model description"); + return false; } - - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - void MecanumDriveController::stopping(const ros::Time& time) + boost::shared_ptr wheel1_urdfJoint(model->getJoint(wheel1_name)); + if(!wheel1_urdfJoint) { - brake(); + ROS_ERROR_STREAM_NAMED(name_, wheel1_name + << " couldn't be retrieved from model description"); + return false; } - - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - void MecanumDriveController::brake() + boost::shared_ptr wheel2_urdfJoint(model->getJoint(wheel2_name)); + if(!wheel2_urdfJoint) + { + ROS_ERROR_STREAM_NAMED(name_, wheel2_name + << " couldn't be retrieved from model description"); + return false; + } + boost::shared_ptr wheel3_urdfJoint(model->getJoint(wheel3_name)); + if(!wheel3_urdfJoint) { - wheel0_jointHandle_.setCommand(0.0); - wheel1_jointHandle_.setCommand(0.0); - wheel2_jointHandle_.setCommand(0.0); - wheel3_jointHandle_.setCommand(0.0); + ROS_ERROR_STREAM_NAMED(name_, wheel3_name + << " couldn't be retrieved from model description"); + return false; } - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - void MecanumDriveController::cmdVelCallback(const geometry_msgs::Twist& command) + ROS_INFO_STREAM("wheel0 to origin: " << wheel0_urdfJoint->parent_to_joint_origin_transform.position.x << "," + << wheel0_urdfJoint->parent_to_joint_origin_transform.position.y << ", " + << wheel0_urdfJoint->parent_to_joint_origin_transform.position.z); + ROS_INFO_STREAM("wheel1 to origin: " << wheel1_urdfJoint->parent_to_joint_origin_transform.position.x << "," + << wheel1_urdfJoint->parent_to_joint_origin_transform.position.y << ", " + << wheel1_urdfJoint->parent_to_joint_origin_transform.position.z); + ROS_INFO_STREAM("wheel2 to origin: " << wheel2_urdfJoint->parent_to_joint_origin_transform.position.x << "," + << wheel2_urdfJoint->parent_to_joint_origin_transform.position.y << ", " + << wheel2_urdfJoint->parent_to_joint_origin_transform.position.z); + ROS_INFO_STREAM("wheel3 to origin: " << wheel3_urdfJoint->parent_to_joint_origin_transform.position.x << "," + << wheel3_urdfJoint->parent_to_joint_origin_transform.position.y << ", " + << wheel3_urdfJoint->parent_to_joint_origin_transform.position.z); + + double wheel0_x = wheel0_urdfJoint->parent_to_joint_origin_transform.position.x; + double wheel0_y = wheel0_urdfJoint->parent_to_joint_origin_transform.position.y; + double wheel1_x = wheel1_urdfJoint->parent_to_joint_origin_transform.position.x; + double wheel1_y = wheel1_urdfJoint->parent_to_joint_origin_transform.position.y; + double wheel2_x = wheel2_urdfJoint->parent_to_joint_origin_transform.position.x; + double wheel2_y = wheel2_urdfJoint->parent_to_joint_origin_transform.position.y; + double wheel3_x = wheel3_urdfJoint->parent_to_joint_origin_transform.position.x; + double wheel3_y = wheel3_urdfJoint->parent_to_joint_origin_transform.position.y; + + wheels_k_ = (-(-wheel0_x - wheel0_y) - (wheel1_x - wheel1_y) + (-wheel2_x - wheel2_y) + (wheel3_x - wheel3_y)) / 4.0; + + // Get wheels radius + double wheel0_radius = 0.0; + double wheel1_radius = 0.0; + double wheel2_radius = 0.0; + double wheel3_radius = 0.0; + + if (!getWheelRadius(model, model->getLink(wheel0_urdfJoint->child_link_name), wheel0_radius) || + !getWheelRadius(model, model->getLink(wheel1_urdfJoint->child_link_name), wheel1_radius) || + !getWheelRadius(model, model->getLink(wheel2_urdfJoint->child_link_name), wheel2_radius) || + !getWheelRadius(model, model->getLink(wheel3_urdfJoint->child_link_name), wheel3_radius)) { - if(isRunning()) - { - command_struct_.ang = command.angular.z; - command_struct_.linX = command.linear.x; - command_struct_.linY = command.linear.y; - 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_.linX << ", " - << "Lin: " << command_struct_.linY << ", " - << "Stamp: " << command_struct_.stamp); - } - else - { - ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running."); - } + ROS_ERROR_STREAM_NAMED(name_, "Couldn't retrieve wheels' radius"); + return false; } - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - bool MecanumDriveController::setWheelParamsFromUrdf(ros::NodeHandle& root_nh, - const std::string& wheel0_name, - const std::string& wheel1_name, - const std::string& wheel2_name, - const std::string& wheel3_name) + if (abs(wheel0_radius - wheel1_radius) > 1e-3 || + abs(wheel0_radius - wheel2_radius) > 1e-3 || + abs(wheel0_radius - wheel3_radius) > 1e-3) { - // Parse robot description - const std::string model_param_name = "robot_description"; - bool res = root_nh.hasParam(model_param_name); - std::string robot_model_str=""; - if(!res || !root_nh.getParam(model_param_name,robot_model_str)) - { - ROS_ERROR_NAMED(name_, "Robot descripion couldn't be retrieved from param server."); - return false; - } + ROS_ERROR_STREAM_NAMED(name_, "Wheels radius are not egual"); + return false; + } - boost::shared_ptr model(urdf::parseURDF(robot_model_str)); + wheels_radius_ = wheel0_radius; - // Get wheels position and compute parameter k_ (used in mecanum wheels IK). - boost::shared_ptr wheel0_urdfJoint(model->getJoint(wheel0_name)); - if(!wheel0_urdfJoint) - { - ROS_ERROR_STREAM_NAMED(name_, wheel0_name - << " couldn't be retrieved from model description"); - return false; - } - boost::shared_ptr wheel1_urdfJoint(model->getJoint(wheel1_name)); - if(!wheel1_urdfJoint) - { - ROS_ERROR_STREAM_NAMED(name_, wheel1_name - << " couldn't be retrieved from model description"); - return false; - } - boost::shared_ptr wheel2_urdfJoint(model->getJoint(wheel2_name)); - if(!wheel2_urdfJoint) - { - ROS_ERROR_STREAM_NAMED(name_, wheel2_name - << " couldn't be retrieved from model description"); - return false; - } - boost::shared_ptr wheel3_urdfJoint(model->getJoint(wheel3_name)); - if(!wheel3_urdfJoint) - { - ROS_ERROR_STREAM_NAMED(name_, wheel3_name - << " couldn't be retrieved from model description"); - return false; - } + ROS_INFO_STREAM("Wheel radius: " << wheels_radius_); - ROS_INFO_STREAM("wheel0 to origin: " << wheel0_urdfJoint->parent_to_joint_origin_transform.position.x << "," - << wheel0_urdfJoint->parent_to_joint_origin_transform.position.y << ", " - << wheel0_urdfJoint->parent_to_joint_origin_transform.position.z); - ROS_INFO_STREAM("wheel1 to origin: " << wheel1_urdfJoint->parent_to_joint_origin_transform.position.x << "," - << wheel1_urdfJoint->parent_to_joint_origin_transform.position.y << ", " - << wheel1_urdfJoint->parent_to_joint_origin_transform.position.z); - ROS_INFO_STREAM("wheel2 to origin: " << wheel2_urdfJoint->parent_to_joint_origin_transform.position.x << "," - << wheel2_urdfJoint->parent_to_joint_origin_transform.position.y << ", " - << wheel2_urdfJoint->parent_to_joint_origin_transform.position.z); - ROS_INFO_STREAM("wheel3 to origin: " << wheel3_urdfJoint->parent_to_joint_origin_transform.position.x << "," - << wheel3_urdfJoint->parent_to_joint_origin_transform.position.y << ", " - << wheel3_urdfJoint->parent_to_joint_origin_transform.position.z); - - double wheel0_x = wheel0_urdfJoint->parent_to_joint_origin_transform.position.x; - double wheel0_y = wheel0_urdfJoint->parent_to_joint_origin_transform.position.y; - double wheel1_x = wheel1_urdfJoint->parent_to_joint_origin_transform.position.x; - double wheel1_y = wheel1_urdfJoint->parent_to_joint_origin_transform.position.y; - double wheel2_x = wheel2_urdfJoint->parent_to_joint_origin_transform.position.x; - double wheel2_y = wheel2_urdfJoint->parent_to_joint_origin_transform.position.y; - double wheel3_x = wheel3_urdfJoint->parent_to_joint_origin_transform.position.x; - double wheel3_y = wheel3_urdfJoint->parent_to_joint_origin_transform.position.y; - - wheels_k_ = (-(-wheel0_x - wheel0_y) - (wheel1_x - wheel1_y) + (-wheel2_x - wheel2_y) + (wheel3_x - wheel3_y)) / 4.0; - - // Get wheels radius - double wheel0_radius = 0.0; - double wheel1_radius = 0.0; - double wheel2_radius = 0.0; - double wheel3_radius = 0.0; - - if (!getWheelRadius(model, model->getLink(wheel0_urdfJoint->child_link_name), wheel0_radius) || - !getWheelRadius(model, model->getLink(wheel1_urdfJoint->child_link_name), wheel1_radius) || - !getWheelRadius(model, model->getLink(wheel2_urdfJoint->child_link_name), wheel2_radius) || - !getWheelRadius(model, model->getLink(wheel3_urdfJoint->child_link_name), wheel3_radius)) - { - ROS_ERROR_STREAM_NAMED(name_, "Couldn't retrieve wheels' radius"); - return false; - } + // Set wheel params for the odometry computation + odometry_.setWheelsParams(wheels_k_, wheels_radius_); - if (abs(wheel0_radius - wheel1_radius) > 1e-3 || - abs(wheel0_radius - wheel2_radius) > 1e-3 || - abs(wheel0_radius - wheel3_radius) > 1e-3) - { - ROS_ERROR_STREAM_NAMED(name_, "Wheels radius are not egual"); - return false; - } - - wheels_radius_ = wheel0_radius; + return true; +} - ROS_INFO_STREAM("Wheel radius: " << wheels_radius_); +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +bool MecanumDriveController::getWheelRadius(const boost::shared_ptr model, const boost::shared_ptr& wheel_link, double& wheel_radius) +{ + boost::shared_ptr radius_link = wheel_link; - // Set wheel params for the odometry computation - odometry_.setWheelsParams(wheels_k_, wheels_radius_); + if (use_realigned_roller_joints_) + { + // This mode is used when the mecanum wheels are simulated and we use realigned rollers to mimic mecanum wheels. + const boost::shared_ptr& roller_joint = radius_link->child_joints[0]; + if(!roller_joint) + { + ROS_ERROR_STREAM_NAMED(name_, "No roller joint could be retrieved for wheel : " << wheel_link->name << ". Are you sure mecanum wheels are simulated using realigned rollers?"); + return false; + } - return true; + radius_link = model->getLink(roller_joint->child_link_name); + if(!radius_link) + { + ROS_ERROR_STREAM_NAMED(name_, "No roller link could be retrieved for wheel : " << wheel_link->name << ". Are you sure mecanum wheels are simulated using realigned rollers?"); + return false; + } } - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - bool MecanumDriveController::getWheelRadius(const boost::shared_ptr model, const boost::shared_ptr& wheel_link, double& wheel_radius) + if(!isCylinderOrSphere(radius_link)) { - boost::shared_ptr radius_link = wheel_link; - - if (use_realigned_roller_joints_) - { - // This mode is used when the mecanum wheels are simulated and we use realigned rollers to mimic mecanum wheels. - const boost::shared_ptr& roller_joint = radius_link->child_joints[0]; - if(!roller_joint) - { - ROS_ERROR_STREAM_NAMED(name_, "No roller joint could be retrieved for wheel : " << wheel_link->name << ". Are you sure mecanum wheels are simulated using realigned rollers?"); - return false; - } - - radius_link = model->getLink(roller_joint->child_link_name); - if(!radius_link) - { - ROS_ERROR_STREAM_NAMED(name_, "No roller link could be retrieved for wheel : " << wheel_link->name << ". Are you sure mecanum wheels are simulated using realigned rollers?"); - return false; - } - } - - if(!isCylinderOrSphere(radius_link)) - { - ROS_ERROR_STREAM("Wheel link " << radius_link->name << " is NOT modeled as a cylinder!"); - return false; - } + ROS_ERROR_STREAM("Wheel link " << radius_link->name << " is NOT modeled as a cylinder!"); + return false; + } - if (radius_link->collision->geometry->type == urdf::Geometry::CYLINDER) - wheel_radius = (static_cast(radius_link->collision->geometry.get()))->radius; - else - wheel_radius = (static_cast(radius_link->collision->geometry.get()))->radius; + if (radius_link->collision->geometry->type == urdf::Geometry::CYLINDER) + wheel_radius = (static_cast(radius_link->collision->geometry.get()))->radius; + else + wheel_radius = (static_cast(radius_link->collision->geometry.get()))->radius; - return true; - } + return true; +} - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - void MecanumDriveController::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(controller_nh, "odom", 100)); - odom_pub_->msg_.header.frame_id = "odom"; - odom_pub_->msg_.child_frame_id = base_frame_id_; - odom_pub_->msg_.pose.pose.position.z = 0; - odom_pub_->msg_.pose.covariance = boost::assign::list_of - (static_cast(pose_cov_list[0])) (0) (0) (0) (0) (0) - (0) (static_cast(pose_cov_list[1])) (0) (0) (0) (0) - (0) (0) (static_cast(pose_cov_list[2])) (0) (0) (0) - (0) (0) (0) (static_cast(pose_cov_list[3])) (0) (0) - (0) (0) (0) (0) (static_cast(pose_cov_list[4])) (0) - (0) (0) (0) (0) (0) (static_cast(pose_cov_list[5])); - odom_pub_->msg_.twist.twist.linear.y = 0; - odom_pub_->msg_.twist.twist.linear.z = 0; - odom_pub_->msg_.twist.twist.angular.x = 0; - odom_pub_->msg_.twist.twist.angular.y = 0; - odom_pub_->msg_.twist.covariance = boost::assign::list_of - (static_cast(twist_cov_list[0])) (0) (0) (0) (0) (0) - (0) (static_cast(twist_cov_list[1])) (0) (0) (0) (0) - (0) (0) (static_cast(twist_cov_list[2])) (0) (0) (0) - (0) (0) (0) (static_cast(twist_cov_list[3])) (0) (0) - (0) (0) (0) (0) (static_cast(twist_cov_list[4])) (0) - (0) (0) (0) (0) (0) (static_cast(twist_cov_list[5])); - tf_odom_pub_.reset(new realtime_tools::RealtimePublisher(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"; - } +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void MecanumDriveController::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(controller_nh, "odom", 100)); + odom_pub_->msg_.header.frame_id = "odom"; + odom_pub_->msg_.child_frame_id = base_frame_id_; + odom_pub_->msg_.pose.pose.position.z = 0; + odom_pub_->msg_.pose.covariance = boost::assign::list_of + (static_cast(pose_cov_list[0])) (0) (0) (0) (0) (0) + (0) (static_cast(pose_cov_list[1])) (0) (0) (0) (0) + (0) (0) (static_cast(pose_cov_list[2])) (0) (0) (0) + (0) (0) (0) (static_cast(pose_cov_list[3])) (0) (0) + (0) (0) (0) (0) (static_cast(pose_cov_list[4])) (0) + (0) (0) (0) (0) (0) (static_cast(pose_cov_list[5])); + odom_pub_->msg_.twist.twist.linear.y = 0; + odom_pub_->msg_.twist.twist.linear.z = 0; + odom_pub_->msg_.twist.twist.angular.x = 0; + odom_pub_->msg_.twist.twist.angular.y = 0; + odom_pub_->msg_.twist.covariance = boost::assign::list_of + (static_cast(twist_cov_list[0])) (0) (0) (0) (0) (0) + (0) (static_cast(twist_cov_list[1])) (0) (0) (0) (0) + (0) (0) (static_cast(twist_cov_list[2])) (0) (0) (0) + (0) (0) (0) (static_cast(twist_cov_list[3])) (0) (0) + (0) (0) (0) (0) (static_cast(twist_cov_list[4])) (0) + (0) (0) (0) (0) (0) (static_cast(twist_cov_list[5])); + tf_odom_pub_.reset(new realtime_tools::RealtimePublisher(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"; +} } // namespace mecanum_drive_controller diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index ce9d31689..1f9d464a1 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -45,102 +45,103 @@ namespace mecanum_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) - , linearX_(0.0) - , linearY_(0.0) - , angular_(0.0) - , wheels_k_(0.0) - , wheels_radius_(0.0) - , velocity_rolling_window_size_(velocity_rolling_window_size) - , linearX_acc_(RollingWindow::window_size = velocity_rolling_window_size) - , linearY_acc_(RollingWindow::window_size = velocity_rolling_window_size) - , angular_acc_(RollingWindow::window_size = velocity_rolling_window_size) - , integrate_fun_(boost::bind(&Odometry::integrateExact, this, _1, _2, _3)) - { - } - - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - void Odometry::init(const ros::Time& time) - { - // Reset accumulators: - linearX_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); - linearY_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); - angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); - - // Reset timestamp: - timestamp_ = time; - } - - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - bool Odometry::update(double wheel0_vel, double wheel1_vel, double wheel2_vel, double wheel3_vel, const ros::Time &time) - { - /// 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; - - /// Compute forward kinematics (i.e. compute mobile robot's twist out of its wheels velocities): - /// Note: we use the IK of the mecanum wheels which we invert using a pseudo-inverse. - double linearX = 0.25 * wheels_radius_ * ( wheel0_vel + wheel1_vel + wheel2_vel + wheel3_vel); - double linearY = 0.25 * wheels_radius_ * (-wheel0_vel + wheel1_vel - wheel2_vel + wheel3_vel); - double angular = 0.25 * wheels_radius_ / wheels_k_ * (-wheel0_vel - wheel1_vel + wheel2_vel + wheel3_vel); - - linearX *= dt; - linearY *= dt; - angular *= dt; - - /// Integrate odometry: - integrate_fun_(linearX, linearY, angular); - - /// Estimate speeds using a rolling mean to filter them out: - linearX_acc_(linearX/dt); - linearY_acc_(linearY/dt); - angular_acc_(angular/dt); - - linearX_ = bacc::rolling_mean(linearX_acc_); - linearY_ = bacc::rolling_mean(linearY_acc_); - angular_ = bacc::rolling_mean(angular_acc_); - - return true; - } - - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - void Odometry::updateOpenLoop(double linearX, double linearY, double angular, const ros::Time &time) - { - /// Save last linear and angular velocity: - linearX_ = linearX; - linearY_ = linearY; - angular_ = angular; - - /// Integrate odometry: - const double dt = (time - timestamp_).toSec(); - timestamp_ = time; - integrate_fun_(linearX * dt, linearY * dt, angular * dt); - } - - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - void Odometry::setWheelsParams(double wheels_k, double wheels_radius) - { - wheels_k_ = wheels_k; - - wheels_radius_ = wheels_radius; - } - - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - void Odometry::integrateExact(double linearX, double linearY, double angular) - { - heading_ += angular; - x_ += linearX; - y_ += linearY; - } + +namespace bacc = boost::accumulators; + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +Odometry::Odometry(size_t velocity_rolling_window_size) +: timestamp_(0.0) +, x_(0.0) +, y_(0.0) +, heading_(0.0) +, linearX_(0.0) +, linearY_(0.0) +, angular_(0.0) +, wheels_k_(0.0) +, wheels_radius_(0.0) +, velocity_rolling_window_size_(velocity_rolling_window_size) +, linearX_acc_(RollingWindow::window_size = velocity_rolling_window_size) +, linearY_acc_(RollingWindow::window_size = velocity_rolling_window_size) +, angular_acc_(RollingWindow::window_size = velocity_rolling_window_size) +, integrate_fun_(boost::bind(&Odometry::integrateExact, this, _1, _2, _3)) +{ +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void Odometry::init(const ros::Time& time) +{ + // Reset accumulators: + linearX_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); + linearY_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); + angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); + + // Reset timestamp: + timestamp_ = time; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +bool Odometry::update(double wheel0_vel, double wheel1_vel, double wheel2_vel, double wheel3_vel, const ros::Time &time) +{ + /// 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; + + /// Compute forward kinematics (i.e. compute mobile robot's twist out of its wheels velocities): + /// Note: we use the IK of the mecanum wheels which we invert using a pseudo-inverse. + double linearX = 0.25 * wheels_radius_ * ( wheel0_vel + wheel1_vel + wheel2_vel + wheel3_vel); + double linearY = 0.25 * wheels_radius_ * (-wheel0_vel + wheel1_vel - wheel2_vel + wheel3_vel); + double angular = 0.25 * wheels_radius_ / wheels_k_ * (-wheel0_vel - wheel1_vel + wheel2_vel + wheel3_vel); + + linearX *= dt; + linearY *= dt; + angular *= dt; + + /// Integrate odometry: + integrate_fun_(linearX, linearY, angular); + + /// Estimate speeds using a rolling mean to filter them out: + linearX_acc_(linearX/dt); + linearY_acc_(linearY/dt); + angular_acc_(angular/dt); + + linearX_ = bacc::rolling_mean(linearX_acc_); + linearY_ = bacc::rolling_mean(linearY_acc_); + angular_ = bacc::rolling_mean(angular_acc_); + + return true; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void Odometry::updateOpenLoop(double linearX, double linearY, double angular, const ros::Time &time) +{ + /// Save last linear and angular velocity: + linearX_ = linearX; + linearY_ = linearY; + angular_ = angular; + + /// Integrate odometry: + const double dt = (time - timestamp_).toSec(); + timestamp_ = time; + integrate_fun_(linearX * dt, linearY * dt, angular * dt); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void Odometry::setWheelsParams(double wheels_k, double wheels_radius) +{ + wheels_k_ = wheels_k; + + wheels_radius_ = wheels_radius; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void Odometry::integrateExact(double linearX, double linearY, double angular) +{ + heading_ += angular; + x_ += linearX; + y_ += linearY; +} } // namespace mecanum_drive_controller From f580175f50329cba2cf976d864b1bd4b544151fc Mon Sep 17 00:00:00 2001 From: Antoine Rennuit Date: Wed, 29 Oct 2014 19:07:40 +0100 Subject: [PATCH 10/21] Integrate the odometry pose in the /odom frame, because: * the odometry pose is expressed in the odom frame (as a ros convention) * the odometry twist is a body twist (as a ros convention) * the input desired twist is also a body twist (as a ros convention) --- .../src/mecanum_drive_controller.cpp | 3 ++- mecanum_drive_controller/src/odometry.cpp | 16 +++++++++++++--- 2 files changed, 15 insertions(+), 4 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 36e501d01..1543720ee 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -257,7 +257,8 @@ void MecanumDriveController::update(const ros::Time& time, const ros::Duration& limiter_ang_.limit(curr_cmd.ang, last_cmd_.ang, cmd_dt); last_cmd_ = curr_cmd; - // Compute wheels velocities: + // Compute wheels velocities (this is the actual ik): + // NOTE: the input desired twist (from topic /cmd_vel) is a body twist. const double w0_vel = 1.0 / wheels_radius_ * (curr_cmd.linX - curr_cmd.linY - wheels_k_ * curr_cmd.ang); const double w1_vel = 1.0 / wheels_radius_ * (curr_cmd.linX + curr_cmd.linY - wheels_k_ * curr_cmd.ang); const double w2_vel = 1.0 / wheels_radius_ * (curr_cmd.linX - curr_cmd.linY + wheels_k_ * curr_cmd.ang); diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index 1f9d464a1..1ee4c9d40 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -41,6 +41,8 @@ #include +#include + #include namespace mecanum_drive_controller @@ -89,7 +91,7 @@ bool Odometry::update(double wheel0_vel, double wheel1_vel, double wheel2_vel, d timestamp_ = time; - /// Compute forward kinematics (i.e. compute mobile robot's twist out of its wheels velocities): + /// Compute forward kinematics (i.e. compute mobile robot's body twist out of its wheels velocities): /// Note: we use the IK of the mecanum wheels which we invert using a pseudo-inverse. double linearX = 0.25 * wheels_radius_ * ( wheel0_vel + wheel1_vel + wheel2_vel + wheel3_vel); double linearY = 0.25 * wheels_radius_ * (-wheel0_vel + wheel1_vel - wheel2_vel + wheel3_vel); @@ -139,9 +141,17 @@ void Odometry::setWheelsParams(double wheels_k, double wheels_radius) //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void Odometry::integrateExact(double linearX, double linearY, double angular) { + /// Integrate angular velocity. heading_ += angular; - x_ += linearX; - y_ += linearY; + + /// The odometry pose should be published in the /odom frame (unlike the odometry twist which is a body twist). + /// Project the twist in the odometry basis (we cannot integrate linearX, linearY, angular 'as are' because they correspond to a body twist). + tf::Matrix3x3 R_m_odom = tf::Matrix3x3(tf::createQuaternionFromYaw(heading_)); + tf::Vector3 vel_inOdom = R_m_odom * tf::Vector3(linearX, linearY, 0.0); + + /// Integrate linear velocity. + x_ += vel_inOdom.x(); + y_ += vel_inOdom.y(); } } // namespace mecanum_drive_controller From 3a1eab5374f49eb9cd2ad49387c54410e870a715 Mon Sep 17 00:00:00 2001 From: Antoine Rennuit Date: Mon, 24 Nov 2014 15:17:31 +0100 Subject: [PATCH 11/21] [mecanum_drive] Removed odometry's velocity filtering (it introduced delay in the reported velocity). --- .../mecanum_drive_controller.h | 2 +- .../src/mecanum_drive_controller.cpp | 10 ++++--- mecanum_drive_controller/src/odometry.cpp | 30 +++++++------------ 3 files changed, 17 insertions(+), 25 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h index 000105d32..f04197872 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h @@ -192,7 +192,7 @@ class MecanumDriveController * \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); + void setupRtPublishersMsg(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh); }; diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 1543720ee..e74f1ae4b 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -174,7 +174,7 @@ bool MecanumDriveController::init(hardware_interface::VelocityJointInterface* hw if (!setWheelParamsFromUrdf(root_nh, wheel0_name, wheel1_name, wheel2_name, wheel3_name)) return false; - setOdomPubFields(root_nh, controller_nh); + setupRtPublishersMsg(root_nh, controller_nh); sub_command_ = controller_nh.subscribe("cmd_vel", 1, &MecanumDriveController::cmdVelCallback, this); @@ -464,9 +464,9 @@ bool MecanumDriveController::getWheelRadius(const boost::shared_ptr(controller_nh, "odom", 100)); odom_pub_->msg_.header.frame_id = "odom"; odom_pub_->msg_.child_frame_id = base_frame_id_; @@ -504,6 +504,8 @@ void MecanumDriveController::setOdomPubFields(ros::NodeHandle& root_nh, ros::Nod (0) (0) (0) (static_cast(twist_cov_list[3])) (0) (0) (0) (0) (0) (0) (static_cast(twist_cov_list[4])) (0) (0) (0) (0) (0) (0) (static_cast(twist_cov_list[5])); + + // Setup tf msg. tf_odom_pub_.reset(new realtime_tools::RealtimePublisher(root_nh, "/tf", 100)); tf_odom_pub_->msg_.transforms.resize(1); tf_odom_pub_->msg_.transforms[0].transform.translation.z = 0.0; diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index 1ee4c9d40..a45631bc2 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -92,26 +92,16 @@ bool Odometry::update(double wheel0_vel, double wheel1_vel, double wheel2_vel, d timestamp_ = time; /// Compute forward kinematics (i.e. compute mobile robot's body twist out of its wheels velocities): - /// Note: we use the IK of the mecanum wheels which we invert using a pseudo-inverse. - double linearX = 0.25 * wheels_radius_ * ( wheel0_vel + wheel1_vel + wheel2_vel + wheel3_vel); - double linearY = 0.25 * wheels_radius_ * (-wheel0_vel + wheel1_vel - wheel2_vel + wheel3_vel); - double angular = 0.25 * wheels_radius_ / wheels_k_ * (-wheel0_vel - wheel1_vel + wheel2_vel + wheel3_vel); - - linearX *= dt; - linearY *= dt; - angular *= dt; - - /// Integrate odometry: - integrate_fun_(linearX, linearY, angular); - - /// Estimate speeds using a rolling mean to filter them out: - linearX_acc_(linearX/dt); - linearY_acc_(linearY/dt); - angular_acc_(angular/dt); - - linearX_ = bacc::rolling_mean(linearX_acc_); - linearY_ = bacc::rolling_mean(linearY_acc_); - angular_ = bacc::rolling_mean(angular_acc_); + /// NOTE: we use the IK of the mecanum wheels which we invert using a pseudo-inverse. + /// NOTE: in the diff drive the velocity is filtered out, but we prefer to return it raw and let the user perform + /// post-processing at will. We prefer this way of doing as filtering introduces delay (which makes it + /// difficult to interpret and compare behavior curves). + linearX_ = 0.25 * wheels_radius_ * ( wheel0_vel + wheel1_vel + wheel2_vel + wheel3_vel); + linearY_ = 0.25 * wheels_radius_ * (-wheel0_vel + wheel1_vel - wheel2_vel + wheel3_vel); + angular_ = 0.25 * wheels_radius_ / wheels_k_ * (-wheel0_vel - wheel1_vel + wheel2_vel + wheel3_vel); + + /// Integrate odometry. + integrate_fun_(linearX_ * dt, linearY_ * dt, angular_ * dt); return true; } From 1ae16bf88723af0b617368ce162fe9cba894d8b3 Mon Sep 17 00:00:00 2001 From: Antoine Rennuit Date: Thu, 25 Jun 2015 16:16:23 +0200 Subject: [PATCH 12/21] [vrep_scene] The definition of the urdf is now close to the scene definition in vrep. --- mecanum_drive_controller/src/mecanum_drive_controller.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index e74f1ae4b..15787ace1 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -103,7 +103,7 @@ bool MecanumDriveController::init(hardware_interface::VelocityJointInterface* hw // Option use_realigned_roller_joints controller_nh.param("use_realigned_roller_joints", use_realigned_roller_joints_, use_realigned_roller_joints_); - ROS_INFO_STREAM_NAMED(name_, "Use the roller's radius rather than the wheel's: " << use_realigned_roller_joints_ << "."); + ROS_INFO_STREAM_NAMED(name_, "Use the roller's radius rather than the wheel's: " << (use_realigned_roller_joints_ ? "true" : "false") << "."); // Get joint names from the parameter server std::string wheel0_name; @@ -125,7 +125,7 @@ bool MecanumDriveController::init(hardware_interface::VelocityJointInterface* hw // 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 " + ROS_INFO_STREAM_NAMED(name_, "Controller state published frequency : " << publish_rate << "Hz."); publish_period_ = ros::Duration(1.0 / publish_rate); @@ -137,10 +137,10 @@ bool MecanumDriveController::init(hardware_interface::VelocityJointInterface* hw << cmd_vel_timeout_ << "s."); 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_); + ROS_INFO_STREAM_NAMED(name_, "Base frame_id : " << base_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")); + ROS_INFO_STREAM_NAMED(name_, "Publishing to tf : " << (enable_odom_tf_?"enabled":"disabled")); // Velocity and acceleration limits: controller_nh.param("linear/x/has_velocity_limits" , limiter_linX_.has_velocity_limits , limiter_linX_.has_velocity_limits ); From a96fbf89f13270163c9ecdaa1a6b7ee143a03fea Mon Sep 17 00:00:00 2001 From: Antoine Rennuit Date: Fri, 3 Jul 2015 18:32:11 +0200 Subject: [PATCH 13/21] [mecanum_drive] Minor fixes (from community peer review on github). --- .../include/mecanum_drive_controller/odometry.h | 11 ++++++----- .../src/mecanum_drive_controller.cpp | 6 +++--- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h index 12d746580..84f111c7f 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h @@ -80,10 +80,10 @@ class Odometry /** * \brief Updates the odometry class with latest wheels position - * \param wheel0_vel Wheel velocity [rad] - * \param wheel1_vel Wheel velocity [rad] - * \param wheel2_vel Wheel velocity [rad] - * \param wheel3_vel Wheel velocity [rad] + * \param wheel0_vel Wheel velocity [rad/s] + * \param wheel1_vel Wheel velocity [rad/s] + * \param wheel2_vel Wheel velocity [rad/s] + * \param wheel3_vel Wheel velocity [rad/s] * \param time Current time * \return true if the odometry is actually updated */ @@ -91,7 +91,8 @@ class Odometry /** * \brief Updates the odometry class with latest velocity command - * \param linearX Linear velocity [m/s] + * \param linearX X component of the linear velocity [m/s] + * \param linearY Y component of the linear velocity [m/s] * \param angular Angular velocity [rad/s] * \param time Current time */ diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 15787ace1..651e83bad 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -408,9 +408,9 @@ bool MecanumDriveController::setWheelParamsFromUrdf(ros::NodeHandle& root_nh, return false; } - if (abs(wheel0_radius - wheel1_radius) > 1e-3 || - abs(wheel0_radius - wheel2_radius) > 1e-3 || - abs(wheel0_radius - wheel3_radius) > 1e-3) + if (fabs(wheel0_radius - wheel1_radius) > 1e-3 || + fabs(wheel0_radius - wheel2_radius) > 1e-3 || + fabs(wheel0_radius - wheel3_radius) > 1e-3) { ROS_ERROR_STREAM_NAMED(name_, "Wheels radius are not egual"); return false; From c7300864374c3f467c6b1f32c9d4f4bfadc7aa95 Mon Sep 17 00:00:00 2001 From: Antoine Rennuit Date: Tue, 7 Jul 2015 15:48:29 +0200 Subject: [PATCH 14/21] [mecanum_drive] Made it possible to offset the base frame with respect to the center frame in the odometry (FK). --- mecanum_drive_controller/README.md | 28 ++- .../mecanum_drive_controller.h | 52 +---- .../mecanum_drive_controller/odometry.h | 210 +++++------------- .../mecanum_drive_controller/speed_limiter.h | 38 ---- .../src/mecanum_drive_controller.cpp | 85 ++----- mecanum_drive_controller/src/odometry.cpp | 121 +++------- .../src/speed_limiter.cpp | 38 ---- 7 files changed, 144 insertions(+), 428 deletions(-) diff --git a/mecanum_drive_controller/README.md b/mecanum_drive_controller/README.md index c488a081d..b18dd86ce 100644 --- a/mecanum_drive_controller/README.md +++ b/mecanum_drive_controller/README.md @@ -1,5 +1,27 @@ -## Mecanum Drive Controller ## +--- MECANUM DRIVE CONTROLLER --- -Controller for a mecanum drive mobile base. -The development of this controller was based on the differential drive controller called diff_drive_controller. +INTRODUCTION + * Controller for a mecanum drive mobile base + * This is a 2D planar controller + * The development of this controller was based on the differential drive controller called diff_drive_controller + +RESTRICTIONS and ASSUMPTIONS + * the controller only works with 4WD mobiles + * the mobile robot is associated a frame called the CENTER frame + - the origin is at the center of the wheels' contact points + - X is front + - Y is left + * the 4 wheels are identical in radius + * the wheels rotation axis is aligned with Y (not -Y) + * the wheels need to have the same parent frame + * the projection of the wheels frame origin on the floor is the contact point of the wheel + +DESCRIPTION + * The velocity command is a body velocity, this body frame is called the BASE frame + - By default the BASE frame = the CENTER frame + * The BASE frame can be offset (with respect to the CENTER frame) using the **base_frame_offset** parameter + - WARNING: the offset of the BASE frame with respect to the CENTER is **not automatically retrieved**, it needs to be indicated by hand + * The odometry computes the location of a frame which is also the BASE frame + - The reference frame of the odometry is the initial position of the BASE frame at t=0 + - This reference frame is called the ODOMETRY frame \ No newline at end of file diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h index f04197872..2c6c7eb51 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h @@ -1,41 +1,3 @@ -/********************************************************************* - * 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 #include #include @@ -52,15 +14,7 @@ namespace mecanum_drive_controller { -/** - * This class makes some assumptions on the model of the robot: - * - the rotation axes of wheels are collinear - * - the wheels are identical in radius - * Additional assumptions to not duplicate information readily available in the URDF: - * - the wheels have the same parent frame - * - a wheel collision geometry is a cylinder in the urdf - * - a wheel joint frame center's vertical projection on the floor must lie within the contact patch - */ +// Check file README.md for restrictions and notes. class MecanumDriveController : public controller_interface::Controller { @@ -102,7 +56,6 @@ class MecanumDriveController /// Odometry related: ros::Duration publish_period_; ros::Time last_state_publish_time_; - bool open_loop_; /// Hardware handles: hardware_interface::JointHandle wheel0_jointHandle_; @@ -139,7 +92,8 @@ class MecanumDriveController double cmd_vel_timeout_; /// Frame to use for the robot base: - std::string base_frame_id_; + std::string base_frame_id_; + double base_frame_offset_[PLANAR_POINT_DIM]; /// Whether to publish odometry to tf or not: bool enable_odom_tf_; diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h index 84f111c7f..2cebc7a1a 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h @@ -1,44 +1,3 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, PAL Robotics, S.L. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the PAL Robotics nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* - * Author: Luca Marchionni - * Author: Bence Magyar - * Author: Enrique Fernández - * Author: Paul Mathieu - */ - #ifndef ODOMETRY_H_ #define ODOMETRY_H_ @@ -48,15 +7,15 @@ #include #include +#define PLANAR_POINT_DIM 3 + namespace mecanum_drive_controller { namespace bacc = boost::accumulators; -/** - * \brief The Odometry class handles odometry readings - * (2D pose and velocity with related timestamp) - */ +/// \brief The Odometry class handles odometry readings +/// (2D pose and velocity with related timestamp) class Odometry { public: @@ -64,99 +23,41 @@ class Odometry /// Integration function, used to integrate the odometry: typedef boost::function 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 - */ + /// \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 wheel0_vel Wheel velocity [rad/s] - * \param wheel1_vel Wheel velocity [rad/s] - * \param wheel2_vel Wheel velocity [rad/s] - * \param wheel3_vel Wheel velocity [rad/s] - * \param time Current time - * \return true if the odometry is actually updated - */ + /// \brief Initialize the odometry + /// \param time Current time + void init(const ros::Time &time, double base_frame_offset[PLANAR_POINT_DIM]); + + /// \brief Updates the odometry class with latest wheels position + /// \param wheel0_vel Wheel velocity [rad/s] + /// \param wheel1_vel Wheel velocity [rad/s] + /// \param wheel2_vel Wheel velocity [rad/s] + /// \param wheel3_vel Wheel velocity [rad/s] + /// \param time Current time + /// \return true if the odometry is actually updated bool update(double wheel0_vel, double wheel1_vel, double wheel2_vel, double wheel3_vel, const ros::Time &time); - /** - * \brief Updates the odometry class with latest velocity command - * \param linearX X component of the linear velocity [m/s] - * \param linearY Y component of the linear velocity [m/s] - * \param angular Angular velocity [rad/s] - * \param time Current time - */ - void updateOpenLoop(double linearX, double linearY, 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 linearX velocity getter - * \return linearX velocity [m/s] - */ - double getLinearX() const - { - return linearX_; - } - - /** - * \brief linearY velocity getter - * \return linearY velocity [m/s] - */ - double getLinearY() const - { - return linearY_; - } - - /** - * \brief angular velocity getter - * \return angular velocity [rad/s] - */ - double getAngular() const - { - return angular_; - } - - /** - * \brief Sets the wheels parameters: mecanum geometric param and radius - * \param wheels_k Wheels geometric param (used in mecanum wheels' ik) [m] - * \param wheels_radius Wheels radius [m] - */ + /// \return position (x component) [m] + double getX() const {return px_b_b0;} + /// \return position (y component) [m] + double getY() const {return py_b_b0;} + /// \return orientation (z component) [m] + double getRz() const {return rz_b_b0;} + /// \return body velocity of the base frame (linear x component) [m/s] + double getVx() const {return vx_Ob_b_b0_b_;} + /// \return body velocity of the base frame (linear y component) [m/s] + double getVy() const {return vy_Ob_b_b0_b_;} + /// \return body velocity of the base frame (angular z component) [m/s] + double getWz() const {return wz_b_b0_b_;} + + /// \brief Sets the wheels parameters: mecanum geometric param and radius + /// \param wheels_k Wheels geometric param (used in mecanum wheels' ik) [m] + /// \param wheels_radius Wheels radius [m] void setWheelsParams(double wheels_k, double wheels_radius); private: @@ -165,26 +66,32 @@ class Odometry typedef bacc::accumulator_set > RollingMeanAcc; typedef bacc::tag::rolling_window RollingWindow; - /** - * \brief Integrates the velocities (linear and angular) using exact method - * \param linearX Linear velocity along X [m] (linear displacement, i.e. m/s * dt) computed by encoders - * \param linearY Linear velocity along Y [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 linearX, double linearY, double angular); - /// Current timestamp: ros::Time timestamp_; - /// Current pose: - double x_; // [m] - double y_; // [m] - double heading_; // [rad] + /// Reference frame (wrt to center frame). + double base_frame_offset_[PLANAR_POINT_DIM]; - /// Current velocity: - double linearX_; // [m/s] - double linearY_; // [m/s] - double angular_; // [rad/s] + /// Current pose: + double px_b_b0; // [m] + double py_b_b0; // [m] + double rz_b_b0; // [rad] + + /// Current velocity. + /// \note The indices meaning is the following: + /// b : base frame + /// c : center frame + /// Ob: origin of the base frame + /// Oc: origin of the center frame + /// b0: initial position if the base frame + /// c0: initial position of the center frame + double vx_Oc_c_c0_c_; // [m/s] + double vy_Oc_c_c0_c_; // [m/s] + double wz_c_c0_c_; // [rad/s] + + double vx_Ob_b_b0_b_; // [m/s] + double vy_Ob_b_b0_b_; // [m/s] + double wz_b_b0_b_; // [rad/s] /// Wheels kinematic parameters [m]: double wheels_k_; @@ -195,9 +102,6 @@ class Odometry RollingMeanAcc linearX_acc_; RollingMeanAcc linearY_acc_; RollingMeanAcc angular_acc_; - - /// Integration funcion, used to integrate the odometry: - IntegrationFunction integrate_fun_; }; } // namespace mecanum_drive_controller diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/speed_limiter.h b/mecanum_drive_controller/include/mecanum_drive_controller/speed_limiter.h index 407451cdf..c5a395478 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/speed_limiter.h +++ b/mecanum_drive_controller/include/mecanum_drive_controller/speed_limiter.h @@ -1,41 +1,3 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, PAL Robotics, S.L. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the PAL Robotics nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* - * Author: Enrique Fernández - */ - #ifndef SPEED_LIMITER_H #define SPEED_LIMITER_H diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 651e83bad..d60435a41 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -1,41 +1,3 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, PAL Robotics, S.L. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the PAL Robotics nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* - * Author: Bence Magyar - */ - #include #include @@ -79,13 +41,13 @@ namespace mecanum_drive_controller //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// MecanumDriveController::MecanumDriveController() - : open_loop_(false) - , command_struct_() + : command_struct_() , use_realigned_roller_joints_(false) , wheels_k_(0.0) , wheels_radius_(0.0) , cmd_vel_timeout_(0.5) , base_frame_id_("base_link") + , base_frame_offset_{0.0, 0.0, 0.0} , enable_odom_tf_(true) , wheel_joints_size_(0) { @@ -129,8 +91,6 @@ bool MecanumDriveController::init(hardware_interface::VelocityJointInterface* hw << publish_rate << "Hz."); publish_period_ = ros::Duration(1.0 / publish_rate); - controller_nh.param("open_loop", open_loop_, open_loop_); - // 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 " @@ -139,6 +99,12 @@ bool MecanumDriveController::init(hardware_interface::VelocityJointInterface* hw controller_nh.param("base_frame_id", base_frame_id_, base_frame_id_); ROS_INFO_STREAM_NAMED(name_, "Base frame_id : " << base_frame_id_); + // TODO: automatically compute the offset of the base frame with respect to the center frame. + controller_nh.param("base_frame_offset/x" , base_frame_offset_[0], base_frame_offset_[0]); + controller_nh.param("base_frame_offset/y" , base_frame_offset_[1], base_frame_offset_[1]); + controller_nh.param("base_frame_offset/theta", base_frame_offset_[2], base_frame_offset_[2]); + ROS_INFO_STREAM_NAMED(name_, "base_frame_offset : " << base_frame_offset_[0] << " " << base_frame_offset_[1] << " " << base_frame_offset_[2]); + controller_nh.param("enable_odom_tf", enable_odom_tf_, enable_odom_tf_); ROS_INFO_STREAM_NAMED(name_, "Publishing to tf : " << (enable_odom_tf_?"enabled":"disabled")); @@ -186,31 +152,24 @@ bool MecanumDriveController::init(hardware_interface::VelocityJointInterface* hw void MecanumDriveController::update(const ros::Time& time, const ros::Duration& period) { // COMPUTE AND PUBLISH ODOMETRY - if (open_loop_) - { - odometry_.updateOpenLoop(last_cmd_.linX, last_cmd_.linY, last_cmd_.ang, time); - } - else - { - double wheel0_vel = wheel0_jointHandle_.getVelocity(); - double wheel1_vel = wheel1_jointHandle_.getVelocity(); - double wheel2_vel = wheel2_jointHandle_.getVelocity(); - double wheel3_vel = wheel3_jointHandle_.getVelocity(); + double wheel0_vel = wheel0_jointHandle_.getVelocity(); + double wheel1_vel = wheel1_jointHandle_.getVelocity(); + double wheel2_vel = wheel2_jointHandle_.getVelocity(); + double wheel3_vel = wheel3_jointHandle_.getVelocity(); - if (std::isnan(wheel0_vel) || std::isnan(wheel1_vel) || std::isnan(wheel2_vel) || std::isnan(wheel3_vel)) - return; + if (std::isnan(wheel0_vel) || std::isnan(wheel1_vel) || std::isnan(wheel2_vel) || std::isnan(wheel3_vel)) + return; - // Estimate twist (using joint information) and integrate - odometry_.update(wheel0_vel, wheel1_vel, wheel2_vel, wheel3_vel, time); - } + // Estimate twist (using joint information) and integrate + odometry_.update(wheel0_vel, wheel1_vel, wheel2_vel, wheel3_vel, 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())); + const geometry_msgs::Quaternion orientation(tf::createQuaternionMsgFromYaw(odometry_.getRz())); // Populate odom message and publish if(odom_pub_->trylock()) @@ -219,9 +178,9 @@ void MecanumDriveController::update(const ros::Time& time, const ros::Duration& 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_.getLinearX(); - odom_pub_->msg_.twist.twist.linear.y = odometry_.getLinearY(); - odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular(); + odom_pub_->msg_.twist.twist.linear.x = odometry_.getVx(); + odom_pub_->msg_.twist.twist.linear.y = odometry_.getVy(); + odom_pub_->msg_.twist.twist.angular.z = odometry_.getWz(); odom_pub_->unlockAndPublish(); } @@ -279,7 +238,7 @@ void MecanumDriveController::starting(const ros::Time& time) // Register starting time used to keep fixed rate last_state_publish_time_ = time; - odometry_.init(time); + odometry_.init(time, base_frame_offset_); } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index a45631bc2..4de376bf1 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -1,44 +1,3 @@ -/********************************************************************* - * 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 #include @@ -53,24 +12,26 @@ namespace bacc = boost::accumulators; //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// Odometry::Odometry(size_t velocity_rolling_window_size) : timestamp_(0.0) -, x_(0.0) -, y_(0.0) -, heading_(0.0) -, linearX_(0.0) -, linearY_(0.0) -, angular_(0.0) +, px_b_b0(0.0) +, py_b_b0(0.0) +, rz_b_b0(0.0) +, vx_Oc_c_c0_c_(0.0) +, vy_Oc_c_c0_c_(0.0) +, wz_c_c0_c_(0.0) +, vx_Ob_b_b0_b_(0.0) +, vy_Ob_b_b0_b_(0.0) +, wz_b_b0_b_(0.0) , wheels_k_(0.0) , wheels_radius_(0.0) , velocity_rolling_window_size_(velocity_rolling_window_size) , linearX_acc_(RollingWindow::window_size = velocity_rolling_window_size) , linearY_acc_(RollingWindow::window_size = velocity_rolling_window_size) , angular_acc_(RollingWindow::window_size = velocity_rolling_window_size) -, integrate_fun_(boost::bind(&Odometry::integrateExact, this, _1, _2, _3)) { } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void Odometry::init(const ros::Time& time) +void Odometry::init(const ros::Time& time, double base_frame_offset[PLANAR_POINT_DIM]) { // Reset accumulators: linearX_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); @@ -79,6 +40,11 @@ void Odometry::init(const ros::Time& time) // Reset timestamp: timestamp_ = time; + + // Base frame offset (wrt to center frame). + base_frame_offset_[0] = base_frame_offset[0]; + base_frame_offset_[1] = base_frame_offset[1]; + base_frame_offset_[2] = base_frame_offset[2]; } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -91,33 +57,36 @@ bool Odometry::update(double wheel0_vel, double wheel1_vel, double wheel2_vel, d timestamp_ = time; - /// Compute forward kinematics (i.e. compute mobile robot's body twist out of its wheels velocities): + /// Compute FK (i.e. compute mobile robot's body twist out of its wheels velocities): /// NOTE: we use the IK of the mecanum wheels which we invert using a pseudo-inverse. + /// NOTE: the mecanum IK gives the body speed at the center frame, we then offset this velocity at the base frame. /// NOTE: in the diff drive the velocity is filtered out, but we prefer to return it raw and let the user perform /// post-processing at will. We prefer this way of doing as filtering introduces delay (which makes it /// difficult to interpret and compare behavior curves). - linearX_ = 0.25 * wheels_radius_ * ( wheel0_vel + wheel1_vel + wheel2_vel + wheel3_vel); - linearY_ = 0.25 * wheels_radius_ * (-wheel0_vel + wheel1_vel - wheel2_vel + wheel3_vel); - angular_ = 0.25 * wheels_radius_ / wheels_k_ * (-wheel0_vel - wheel1_vel + wheel2_vel + wheel3_vel); + vx_Oc_c_c0_c_ = 0.25 * wheels_radius_ * ( wheel0_vel + wheel1_vel + wheel2_vel + wheel3_vel); + vy_Oc_c_c0_c_ = 0.25 * wheels_radius_ * (-wheel0_vel + wheel1_vel - wheel2_vel + wheel3_vel); + wz_c_c0_c_ = 0.25 * wheels_radius_ / wheels_k_ * (-wheel0_vel - wheel1_vel + wheel2_vel + wheel3_vel); - /// Integrate odometry. - integrate_fun_(linearX_ * dt, linearY_ * dt, angular_ * dt); + tf::Matrix3x3 R_c_b = tf::Matrix3x3(tf::createQuaternionFromYaw(-base_frame_offset_[2])); + tf::Vector3 v_Oc_c_c0_b = R_c_b * tf::Vector3(vx_Oc_c_c0_c_, vy_Oc_c_c0_c_, 0.0); + tf::Vector3 Oc_b = R_c_b * tf::Vector3(-base_frame_offset_[0], -base_frame_offset_[1], 0.0); - return true; -} + vx_Ob_b_b0_b_ = v_Oc_c_c0_b.x() + Oc_b.y() * wz_c_c0_c_; + vy_Ob_b_b0_b_ = v_Oc_c_c0_b.y() - Oc_b.x() * wz_c_c0_c_; + wz_b_b0_b_ = wz_c_c0_c_; -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void Odometry::updateOpenLoop(double linearX, double linearY, double angular, const ros::Time &time) -{ - /// Save last linear and angular velocity: - linearX_ = linearX; - linearY_ = linearY; - angular_ = angular; + /// Integration. + /// NOTE: the position is expressed in the odometry frame (frame b0), unlike the twist which is expressed in the body + /// frame (frame b). + rz_b_b0 += wz_b_b0_b_ * dt; - /// Integrate odometry: - const double dt = (time - timestamp_).toSec(); - timestamp_ = time; - integrate_fun_(linearX * dt, linearY * dt, angular * dt); + tf::Matrix3x3 R_b_b0 = tf::Matrix3x3(tf::createQuaternionFromYaw(rz_b_b0)); + tf::Vector3 vx_Ob_b_b0_b0 = R_b_b0 * tf::Vector3(vx_Ob_b_b0_b_, vy_Ob_b_b0_b_, 0.0); + + px_b_b0 += vx_Ob_b_b0_b0.x() * dt; + py_b_b0 += vx_Ob_b_b0_b0.y() * dt; + + return true; } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -128,20 +97,4 @@ void Odometry::setWheelsParams(double wheels_k, double wheels_radius) wheels_radius_ = wheels_radius; } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void Odometry::integrateExact(double linearX, double linearY, double angular) -{ - /// Integrate angular velocity. - heading_ += angular; - - /// The odometry pose should be published in the /odom frame (unlike the odometry twist which is a body twist). - /// Project the twist in the odometry basis (we cannot integrate linearX, linearY, angular 'as are' because they correspond to a body twist). - tf::Matrix3x3 R_m_odom = tf::Matrix3x3(tf::createQuaternionFromYaw(heading_)); - tf::Vector3 vel_inOdom = R_m_odom * tf::Vector3(linearX, linearY, 0.0); - - /// Integrate linear velocity. - x_ += vel_inOdom.x(); - y_ += vel_inOdom.y(); -} - } // namespace mecanum_drive_controller diff --git a/mecanum_drive_controller/src/speed_limiter.cpp b/mecanum_drive_controller/src/speed_limiter.cpp index 1eda26930..9eb21e2a7 100644 --- a/mecanum_drive_controller/src/speed_limiter.cpp +++ b/mecanum_drive_controller/src/speed_limiter.cpp @@ -1,41 +1,3 @@ -/********************************************************************* - * 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 #include From bd345d72061b5fe340f31516cb5efa4bc6ea319f Mon Sep 17 00:00:00 2001 From: Antoine Rennuit Date: Tue, 7 Jul 2015 15:49:55 +0200 Subject: [PATCH 15/21] [mecanum_drive] Made it possible to offset the base frame with respect to the center frame in the command (IK side). --- .../mecanum_drive_controller.h | 26 ++-- .../mecanum_drive_controller/odometry.h | 22 +--- .../src/mecanum_drive_controller.cpp | 118 ++++++++---------- mecanum_drive_controller/src/odometry.cpp | 42 +++---- 4 files changed, 82 insertions(+), 126 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h index 2c6c7eb51..7fdab9e5d 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h @@ -64,22 +64,22 @@ class MecanumDriveController hardware_interface::JointHandle wheel3_jointHandle_; /// Velocity command related: - struct Commands + struct Command { - double linX; - double linY; - double ang; + double vx_Ob_b_b0_b; + double vy_Ob_b_b0_b; + double wz_b_b0_b; ros::Time stamp; - Commands() : linX(0.0), linY(0.0), ang(0.0), stamp(0.0) {} + Command() : vx_Ob_b_b0_b(0.0), vy_Ob_b_b0_b(0.0), wz_b_b0_b(0.0), stamp(0.0) {} }; - realtime_tools::RealtimeBuffer command_; - Commands command_struct_; - ros::Subscriber sub_command_; + realtime_tools::RealtimeBuffer command_rt_buffer_; + Command command_; + ros::Subscriber command_sub_; /// Odometry related: boost::shared_ptr > odom_pub_; - boost::shared_ptr > tf_odom_pub_; + boost::shared_ptr > tf_pub_; Odometry odometry_; geometry_msgs::TransformStamped odom_frame_; @@ -101,12 +101,6 @@ class MecanumDriveController /// Number of wheel joints: size_t wheel_joints_size_; - // Speed limiters: - Commands last_cmd_; - SpeedLimiter limiter_linX_; - SpeedLimiter limiter_linY_; - SpeedLimiter limiter_ang_; - private: /** * \brief Brakes the wheels, i.e. sets the velocity to 0 @@ -117,7 +111,7 @@ class MecanumDriveController * \brief Velocity command callback * \param command Velocity command message (twist) */ - void cmdVelCallback(const geometry_msgs::Twist& command); + void commandCb(const geometry_msgs::Twist& command); /** * \brief Sets odometry parameters from the URDF, i.e. the wheel radius and separation diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h index 2cebc7a1a..4e0b2b824 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h @@ -43,11 +43,11 @@ class Odometry bool update(double wheel0_vel, double wheel1_vel, double wheel2_vel, double wheel3_vel, const ros::Time &time); /// \return position (x component) [m] - double getX() const {return px_b_b0;} + double getX() const {return px_b_b0_;} /// \return position (y component) [m] - double getY() const {return py_b_b0;} + double getY() const {return py_b_b0_;} /// \return orientation (z component) [m] - double getRz() const {return rz_b_b0;} + double getRz() const {return rz_b_b0_;} /// \return body velocity of the base frame (linear x component) [m/s] double getVx() const {return vx_Ob_b_b0_b_;} /// \return body velocity of the base frame (linear y component) [m/s] @@ -73,9 +73,9 @@ class Odometry double base_frame_offset_[PLANAR_POINT_DIM]; /// Current pose: - double px_b_b0; // [m] - double py_b_b0; // [m] - double rz_b_b0; // [rad] + double px_b_b0_; // [m] + double py_b_b0_; // [m] + double rz_b_b0_; // [rad] /// Current velocity. /// \note The indices meaning is the following: @@ -85,10 +85,6 @@ class Odometry /// Oc: origin of the center frame /// b0: initial position if the base frame /// c0: initial position of the center frame - double vx_Oc_c_c0_c_; // [m/s] - double vy_Oc_c_c0_c_; // [m/s] - double wz_c_c0_c_; // [rad/s] - double vx_Ob_b_b0_b_; // [m/s] double vy_Ob_b_b0_b_; // [m/s] double wz_b_b0_b_; // [rad/s] @@ -96,12 +92,6 @@ class Odometry /// Wheels kinematic parameters [m]: double wheels_k_; double wheels_radius_; - - /// Rolling mean accumulators for the linar and angular velocities: - size_t velocity_rolling_window_size_; - RollingMeanAcc linearX_acc_; - RollingMeanAcc linearY_acc_; - RollingMeanAcc angular_acc_; }; } // namespace mecanum_drive_controller diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index d60435a41..8ddf06409 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -41,7 +41,7 @@ namespace mecanum_drive_controller //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// MecanumDriveController::MecanumDriveController() - : command_struct_() + : command_() , use_realigned_roller_joints_(false) , wheels_k_(0.0) , wheels_radius_(0.0) @@ -108,28 +108,6 @@ bool MecanumDriveController::init(hardware_interface::VelocityJointInterface* hw controller_nh.param("enable_odom_tf", enable_odom_tf_, enable_odom_tf_); ROS_INFO_STREAM_NAMED(name_, "Publishing to tf : " << (enable_odom_tf_?"enabled":"disabled")); - // Velocity and acceleration limits: - controller_nh.param("linear/x/has_velocity_limits" , limiter_linX_.has_velocity_limits , limiter_linX_.has_velocity_limits ); - controller_nh.param("linear/x/has_acceleration_limits", limiter_linX_.has_acceleration_limits, limiter_linX_.has_acceleration_limits); - controller_nh.param("linear/x/max_velocity" , limiter_linX_.max_velocity , limiter_linX_.max_velocity ); - controller_nh.param("linear/x/min_velocity" , limiter_linX_.min_velocity , -limiter_linX_.max_velocity ); - controller_nh.param("linear/x/max_acceleration" , limiter_linX_.max_acceleration , limiter_linX_.max_acceleration ); - controller_nh.param("linear/x/min_acceleration" , limiter_linX_.min_acceleration , -limiter_linX_.max_acceleration ); - - controller_nh.param("linear/y/has_velocity_limits" , limiter_linY_.has_velocity_limits , limiter_linY_.has_velocity_limits ); - controller_nh.param("linear/y/has_acceleration_limits", limiter_linY_.has_acceleration_limits, limiter_linY_.has_acceleration_limits); - controller_nh.param("linear/y/max_velocity" , limiter_linY_.max_velocity , limiter_linY_.max_velocity ); - controller_nh.param("linear/y/min_velocity" , limiter_linY_.min_velocity , -limiter_linY_.max_velocity ); - controller_nh.param("linear/y/max_acceleration" , limiter_linY_.max_acceleration , limiter_linY_.max_acceleration ); - controller_nh.param("linear/y/min_acceleration" , limiter_linY_.min_acceleration , -limiter_linY_.max_acceleration ); - - 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/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 ); - // Get the joint objects to use in the realtime loop wheel0_jointHandle_ = hw->getHandle(wheel0_name); // throws on failure wheel1_jointHandle_ = hw->getHandle(wheel1_name); // throws on failure @@ -142,7 +120,7 @@ bool MecanumDriveController::init(hardware_interface::VelocityJointInterface* hw setupRtPublishersMsg(root_nh, controller_nh); - sub_command_ = controller_nh.subscribe("cmd_vel", 1, &MecanumDriveController::cmdVelCallback, this); + command_sub_ = controller_nh.subscribe("cmd_vel", 1, &MecanumDriveController::commandCb, this); return true; } @@ -151,7 +129,7 @@ bool MecanumDriveController::init(hardware_interface::VelocityJointInterface* hw //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void MecanumDriveController::update(const ros::Time& time, const ros::Duration& period) { - // COMPUTE AND PUBLISH ODOMETRY + // FORWARD KINEMATICS (odometry). double wheel0_vel = wheel0_jointHandle_.getVelocity(); double wheel1_vel = wheel1_jointHandle_.getVelocity(); double wheel2_vel = wheel2_jointHandle_.getVelocity(); @@ -181,47 +159,50 @@ void MecanumDriveController::update(const ros::Time& time, const ros::Duration& odom_pub_->msg_.twist.twist.linear.x = odometry_.getVx(); odom_pub_->msg_.twist.twist.linear.y = odometry_.getVy(); odom_pub_->msg_.twist.twist.angular.z = odometry_.getWz(); + odom_pub_->unlockAndPublish(); } // Publish tf /odom frame - if (enable_odom_tf_ && tf_odom_pub_->trylock()) + if (enable_odom_tf_ && tf_pub_->trylock()) { - geometry_msgs::TransformStamped& odom_frame = tf_odom_pub_->msg_.transforms[0]; + geometry_msgs::TransformStamped& odom_frame = tf_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(); + + tf_pub_->unlockAndPublish(); } } - // MOVE ROBOT - // Retrieve current velocity command and time step: - Commands curr_cmd = *(command_.readFromRT()); - const double dt = (time - curr_cmd.stamp).toSec(); + // INVERSE KINEMATICS (move robot). // Brake if cmd_vel has timeout: + Command curr_cmd = *(command_rt_buffer_.readFromRT()); + const double dt = (time - curr_cmd.stamp).toSec(); + if (dt > cmd_vel_timeout_) { - curr_cmd.linX = 0.0; - curr_cmd.linY = 0.0; - curr_cmd.ang = 0.0; + curr_cmd.vx_Ob_b_b0_b = 0.0; + curr_cmd.vy_Ob_b_b0_b = 0.0; + curr_cmd.wz_b_b0_b = 0.0; } - // Limit velocities and accelerations: - const double cmd_dt(period.toSec()); - limiter_linX_.limit(curr_cmd.linX, last_cmd_.linX, cmd_dt); - limiter_linY_.limit(curr_cmd.linY, last_cmd_.linY, cmd_dt); - limiter_ang_.limit(curr_cmd.ang, last_cmd_.ang, cmd_dt); - last_cmd_ = curr_cmd; - // Compute wheels velocities (this is the actual ik): // NOTE: the input desired twist (from topic /cmd_vel) is a body twist. - const double w0_vel = 1.0 / wheels_radius_ * (curr_cmd.linX - curr_cmd.linY - wheels_k_ * curr_cmd.ang); - const double w1_vel = 1.0 / wheels_radius_ * (curr_cmd.linX + curr_cmd.linY - wheels_k_ * curr_cmd.ang); - const double w2_vel = 1.0 / wheels_radius_ * (curr_cmd.linX - curr_cmd.linY + wheels_k_ * curr_cmd.ang); - const double w3_vel = 1.0 / wheels_radius_ * (curr_cmd.linX + curr_cmd.linY + wheels_k_ * curr_cmd.ang); + tf::Matrix3x3 R_b_c = tf::Matrix3x3(tf::createQuaternionFromYaw(base_frame_offset_[2])); + tf::Vector3 v_Ob_b_b0_c = R_b_c * tf::Vector3(curr_cmd.vx_Ob_b_b0_b, curr_cmd.vy_Ob_b_b0_b, 0.0); + tf::Vector3 Ob_c = tf::Vector3(base_frame_offset_[0], base_frame_offset_[1], 0.0); + + double vx_Oc_c_c0_c_ = v_Ob_b_b0_c.x() + Ob_c.y() * curr_cmd.wz_b_b0_b; + double vy_Oc_c_c0_c_ = v_Ob_b_b0_c.y() - Ob_c.x() * curr_cmd.wz_b_b0_b; + double wz_c_c0_c_ = curr_cmd.wz_b_b0_b; + + double w0_vel = 1.0 / wheels_radius_ * (vx_Oc_c_c0_c_ - vy_Oc_c_c0_c_ - wheels_k_ * wz_c_c0_c_); + double w1_vel = 1.0 / wheels_radius_ * (vx_Oc_c_c0_c_ + vy_Oc_c_c0_c_ - wheels_k_ * wz_c_c0_c_); + double w2_vel = 1.0 / wheels_radius_ * (vx_Oc_c_c0_c_ - vy_Oc_c_c0_c_ + wheels_k_ * wz_c_c0_c_); + double w3_vel = 1.0 / wheels_radius_ * (vx_Oc_c_c0_c_ + vy_Oc_c_c0_c_ + wheels_k_ * wz_c_c0_c_); // Set wheels velocities: wheel0_jointHandle_.setCommand(w0_vel); @@ -257,26 +238,27 @@ void MecanumDriveController::brake() } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void MecanumDriveController::cmdVelCallback(const geometry_msgs::Twist& command) +void MecanumDriveController::commandCb(const geometry_msgs::Twist& command) { - if(isRunning()) - { - command_struct_.ang = command.angular.z; - command_struct_.linX = command.linear.x; - command_struct_.linY = command.linear.y; - command_struct_.stamp = ros::Time::now(); - command_.writeFromNonRT (command_struct_); + if (!isRunning()) + { + ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running."); + return; + } + + command_.vx_Ob_b_b0_b = command.linear.x; + command_.vy_Ob_b_b0_b = command.linear.y; + command_.wz_b_b0_b = command.angular.z; + + command_.stamp = ros::Time::now(); + command_rt_buffer_.writeFromNonRT(command_); + ROS_DEBUG_STREAM_NAMED(name_, "Added values to command. " - << "Ang: " << command_struct_.ang << ", " - << "Lin: " << command_struct_.linX << ", " - << "Lin: " << command_struct_.linY << ", " - << "Stamp: " << command_struct_.stamp); - } - else - { - ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running."); - } + << "vx_Ob_b_b0_b : " << command_.vx_Ob_b_b0_b << ", " + << "vy_Ob_b_b0_b : " << command_.vy_Ob_b_b0_b << ", " + << "wz_b_b0_b : " << command_.wz_b_b0_b << ", " + << "Stamp : " << command_.stamp); } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -465,11 +447,11 @@ void MecanumDriveController::setupRtPublishersMsg(ros::NodeHandle& root_nh, ros: (0) (0) (0) (0) (0) (static_cast(twist_cov_list[5])); // Setup tf msg. - tf_odom_pub_.reset(new realtime_tools::RealtimePublisher(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"; + tf_pub_.reset(new realtime_tools::RealtimePublisher(root_nh, "/tf", 100)); + tf_pub_->msg_.transforms.resize(1); + tf_pub_->msg_.transforms[0].transform.translation.z = 0.0; + tf_pub_->msg_.transforms[0].child_frame_id = base_frame_id_; + tf_pub_->msg_.transforms[0].header.frame_id = "odom"; } } // namespace mecanum_drive_controller diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index 4de376bf1..5f6075725 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -12,32 +12,22 @@ namespace bacc = boost::accumulators; //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// Odometry::Odometry(size_t velocity_rolling_window_size) : timestamp_(0.0) -, px_b_b0(0.0) -, py_b_b0(0.0) -, rz_b_b0(0.0) -, vx_Oc_c_c0_c_(0.0) -, vy_Oc_c_c0_c_(0.0) -, wz_c_c0_c_(0.0) + +, px_b_b0_(0.0) +, py_b_b0_(0.0) +, rz_b_b0_(0.0) , vx_Ob_b_b0_b_(0.0) , vy_Ob_b_b0_b_(0.0) , wz_b_b0_b_(0.0) + , wheels_k_(0.0) , wheels_radius_(0.0) -, velocity_rolling_window_size_(velocity_rolling_window_size) -, linearX_acc_(RollingWindow::window_size = velocity_rolling_window_size) -, linearY_acc_(RollingWindow::window_size = velocity_rolling_window_size) -, angular_acc_(RollingWindow::window_size = velocity_rolling_window_size) { } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void Odometry::init(const ros::Time& time, double base_frame_offset[PLANAR_POINT_DIM]) { - // Reset accumulators: - linearX_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); - linearY_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); - angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); - // Reset timestamp: timestamp_ = time; @@ -63,28 +53,28 @@ bool Odometry::update(double wheel0_vel, double wheel1_vel, double wheel2_vel, d /// NOTE: in the diff drive the velocity is filtered out, but we prefer to return it raw and let the user perform /// post-processing at will. We prefer this way of doing as filtering introduces delay (which makes it /// difficult to interpret and compare behavior curves). - vx_Oc_c_c0_c_ = 0.25 * wheels_radius_ * ( wheel0_vel + wheel1_vel + wheel2_vel + wheel3_vel); - vy_Oc_c_c0_c_ = 0.25 * wheels_radius_ * (-wheel0_vel + wheel1_vel - wheel2_vel + wheel3_vel); - wz_c_c0_c_ = 0.25 * wheels_radius_ / wheels_k_ * (-wheel0_vel - wheel1_vel + wheel2_vel + wheel3_vel); + double vx_Oc_c_c0_c = 0.25 * wheels_radius_ * ( wheel0_vel + wheel1_vel + wheel2_vel + wheel3_vel); + double vy_Oc_c_c0_c = 0.25 * wheels_radius_ * (-wheel0_vel + wheel1_vel - wheel2_vel + wheel3_vel); + double wz_c_c0_c = 0.25 * wheels_radius_ / wheels_k_ * (-wheel0_vel - wheel1_vel + wheel2_vel + wheel3_vel); tf::Matrix3x3 R_c_b = tf::Matrix3x3(tf::createQuaternionFromYaw(-base_frame_offset_[2])); - tf::Vector3 v_Oc_c_c0_b = R_c_b * tf::Vector3(vx_Oc_c_c0_c_, vy_Oc_c_c0_c_, 0.0); + tf::Vector3 v_Oc_c_c0_b = R_c_b * tf::Vector3(vx_Oc_c_c0_c, vy_Oc_c_c0_c, 0.0); tf::Vector3 Oc_b = R_c_b * tf::Vector3(-base_frame_offset_[0], -base_frame_offset_[1], 0.0); - vx_Ob_b_b0_b_ = v_Oc_c_c0_b.x() + Oc_b.y() * wz_c_c0_c_; - vy_Ob_b_b0_b_ = v_Oc_c_c0_b.y() - Oc_b.x() * wz_c_c0_c_; - wz_b_b0_b_ = wz_c_c0_c_; + vx_Ob_b_b0_b_ = v_Oc_c_c0_b.x() + Oc_b.y() * wz_c_c0_c; + vy_Ob_b_b0_b_ = v_Oc_c_c0_b.y() - Oc_b.x() * wz_c_c0_c; + wz_b_b0_b_ = wz_c_c0_c; /// Integration. /// NOTE: the position is expressed in the odometry frame (frame b0), unlike the twist which is expressed in the body /// frame (frame b). - rz_b_b0 += wz_b_b0_b_ * dt; + rz_b_b0_ += wz_b_b0_b_ * dt; - tf::Matrix3x3 R_b_b0 = tf::Matrix3x3(tf::createQuaternionFromYaw(rz_b_b0)); + tf::Matrix3x3 R_b_b0 = tf::Matrix3x3(tf::createQuaternionFromYaw(rz_b_b0_)); tf::Vector3 vx_Ob_b_b0_b0 = R_b_b0 * tf::Vector3(vx_Ob_b_b0_b_, vy_Ob_b_b0_b_, 0.0); - px_b_b0 += vx_Ob_b_b0_b0.x() * dt; - py_b_b0 += vx_Ob_b_b0_b0.y() * dt; + px_b_b0_ += vx_Ob_b_b0_b0.x() * dt; + py_b_b0_ += vx_Ob_b_b0_b0.y() * dt; return true; } From 188be8fc07e6ea2cace4146321d367a02504a5dd Mon Sep 17 00:00:00 2001 From: Antoine Rennuit Date: Tue, 7 Jul 2015 15:55:15 +0200 Subject: [PATCH 16/21] [mecanum_drive] Get rid of speed_limiter (unused). --- mecanum_drive_controller/CMakeLists.txt | 2 - .../mecanum_drive_controller.h | 1 - .../mecanum_drive_controller/speed_limiter.h | 67 ------------------- .../src/speed_limiter.cpp | 60 ----------------- 4 files changed, 130 deletions(-) delete mode 100644 mecanum_drive_controller/include/mecanum_drive_controller/speed_limiter.h delete mode 100644 mecanum_drive_controller/src/speed_limiter.cpp diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt index 0d1a4c88c..7b8429004 100644 --- a/mecanum_drive_controller/CMakeLists.txt +++ b/mecanum_drive_controller/CMakeLists.txt @@ -14,13 +14,11 @@ find_package(catkin REQUIRED COMPONENTS set(${PROJECT_NAME}_headers include/mecanum_drive_controller/mecanum_drive_controller.h include/mecanum_drive_controller/odometry.h - include/mecanum_drive_controller/speed_limiter.h ) set(${PROJECT_NAME}_sources src/mecanum_drive_controller.cpp src/odometry.cpp - src/speed_limiter.cpp ) catkin_package( diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h index 7fdab9e5d..09e0cfd9d 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h @@ -9,7 +9,6 @@ #include #include -#include namespace mecanum_drive_controller { diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/speed_limiter.h b/mecanum_drive_controller/include/mecanum_drive_controller/speed_limiter.h deleted file mode 100644 index c5a395478..000000000 --- a/mecanum_drive_controller/include/mecanum_drive_controller/speed_limiter.h +++ /dev/null @@ -1,67 +0,0 @@ -#ifndef SPEED_LIMITER_H -#define SPEED_LIMITER_H - -namespace mecanum_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] 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 - */ - SpeedLimiter( - bool has_velocity_limits = false, - bool has_acceleration_limits = false, - double min_velocity = 0.0, - double max_velocity = 0.0, - double min_acceleration = 0.0, - double max_acceleration = 0.0 - ); - - /** - * \brief Limit the velocity and acceleration - * \param [in, out] v Velocity [m/s] - * \param [in] v0 Previous velocity [m/s] - * \param [in] dt Time step [s] - */ - void limit(double& v, double v0, double dt); - - /** - * \brief Limit the velocity - * \param [in, out] v Velocity [m/s] - */ - void 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] - */ - void limit_acceleration(double& v, double v0, double dt); - - public: - // Enable/Disable velocity/acceleration limits: - bool has_velocity_limits; - bool has_acceleration_limits; - - // Velocity limits: - double min_velocity; - double max_velocity; - - // Acceleration limits: - double min_acceleration; - double max_acceleration; - }; - -} // namespace mecanum_drive_controller - -#endif // SPEED_LIMITER_H diff --git a/mecanum_drive_controller/src/speed_limiter.cpp b/mecanum_drive_controller/src/speed_limiter.cpp deleted file mode 100644 index 9eb21e2a7..000000000 --- a/mecanum_drive_controller/src/speed_limiter.cpp +++ /dev/null @@ -1,60 +0,0 @@ -#include - -#include - -template -T clamp(T x, T min, T max) -{ - return std::min(std::max(min, x), max); -} - -namespace mecanum_drive_controller -{ - - SpeedLimiter::SpeedLimiter( - bool has_velocity_limits, - bool has_acceleration_limits, - double min_velocity, - double max_velocity, - double min_acceleration, - double max_acceleration - ) - : has_velocity_limits(has_velocity_limits), - has_acceleration_limits(has_acceleration_limits), - min_velocity(min_velocity), - max_velocity(max_velocity), - min_acceleration(min_acceleration), - max_acceleration(max_acceleration) - { - } - - void SpeedLimiter::limit(double& v, double v0, double dt) - { - limit_velocity(v); - limit_acceleration(v, v0, dt); - } - - void SpeedLimiter::limit_velocity(double& v) - { - if (has_velocity_limits) - { - v = clamp(v, min_velocity, max_velocity); - } - } - - void SpeedLimiter::limit_acceleration(double& v, double v0, double dt) - { - if (has_acceleration_limits) - { - double dv = v - v0; - - const double dv_min = min_acceleration * dt; - const double dv_max = max_acceleration * dt; - - dv = clamp(dv, dv_min, dv_max); - - v = v0 + dv; - } - } - -} // namespace mecanum_drive_controller From d4c141b39e5e64b6ec9ec5dd2b513f07e31a8784 Mon Sep 17 00:00:00 2001 From: Antoine Rennuit Date: Tue, 7 Jul 2015 16:38:59 +0200 Subject: [PATCH 17/21] [mecanum_drive] Added some more details to the readme.md file. --- mecanum_drive_controller/README.md | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/mecanum_drive_controller/README.md b/mecanum_drive_controller/README.md index b18dd86ce..40f80820a 100644 --- a/mecanum_drive_controller/README.md +++ b/mecanum_drive_controller/README.md @@ -14,6 +14,7 @@ RESTRICTIONS and ASSUMPTIONS - Y is left * the 4 wheels are identical in radius * the wheels rotation axis is aligned with Y (not -Y) + - that is the axis of rotation of each wheel is in the Y direction * the wheels need to have the same parent frame * the projection of the wheels frame origin on the floor is the contact point of the wheel @@ -24,4 +25,9 @@ DESCRIPTION - WARNING: the offset of the BASE frame with respect to the CENTER is **not automatically retrieved**, it needs to be indicated by hand * The odometry computes the location of a frame which is also the BASE frame - The reference frame of the odometry is the initial position of the BASE frame at t=0 - - This reference frame is called the ODOMETRY frame \ No newline at end of file + - This reference frame is called the ODOMETRY frame + * The wheels are refered to using an index 0, 1, 2, 3 + - wheel 0: front left wheel + - wheel 1: back left wheel + - wheel 2: back right wheel + - wheel 3: front right wheel \ No newline at end of file From 071c3a8441c0111219222e36e604b714b5fa786f Mon Sep 17 00:00:00 2001 From: Antoine Rennuit Date: Wed, 8 Jul 2015 13:25:25 +0200 Subject: [PATCH 18/21] [mecanum_drive] Minor cleaning (should not change behavior). --- mecanum_drive_controller/src/mecanum_drive_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 8ddf06409..3b08c8b12 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -248,7 +248,7 @@ void MecanumDriveController::commandCb(const geometry_msgs::Twist& command) command_.vx_Ob_b_b0_b = command.linear.x; command_.vy_Ob_b_b0_b = command.linear.y; - command_.wz_b_b0_b = command.angular.z; + command_.wz_b_b0_b = command.angular.z; command_.stamp = ros::Time::now(); command_rt_buffer_.writeFromNonRT(command_); @@ -258,7 +258,7 @@ void MecanumDriveController::commandCb(const geometry_msgs::Twist& command) << "vx_Ob_b_b0_b : " << command_.vx_Ob_b_b0_b << ", " << "vy_Ob_b_b0_b : " << command_.vy_Ob_b_b0_b << ", " << "wz_b_b0_b : " << command_.wz_b_b0_b << ", " - << "Stamp : " << command_.stamp); + << "Stamp : " << command_.stamp); } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// From e49079854aaf935288e3686a2281b07c252b3afc Mon Sep 17 00:00:00 2001 From: Antoine Rennuit Date: Mon, 13 Jul 2015 10:41:21 +0200 Subject: [PATCH 19/21] [vrep_scene] Minor reorganisation (no impact on behavior). --- .../mecanum_drive_controller.h | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h index 09e0cfd9d..b58f55894 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h @@ -52,10 +52,6 @@ class MecanumDriveController private: std::string name_; - /// Odometry related: - ros::Duration publish_period_; - ros::Time last_state_publish_time_; - /// Hardware handles: hardware_interface::JointHandle wheel0_jointHandle_; hardware_interface::JointHandle wheel1_jointHandle_; @@ -77,11 +73,16 @@ class MecanumDriveController ros::Subscriber command_sub_; /// Odometry related: + Odometry odometry_; + boost::shared_ptr > odom_pub_; boost::shared_ptr > tf_pub_; - Odometry odometry_; + geometry_msgs::TransformStamped odom_frame_; + ros::Duration publish_period_; + ros::Time last_state_publish_time_; + /// Wheel radius (assuming it's the same for the left and right wheels): bool use_realigned_roller_joints_; double wheels_k_; // wheels geometric param used in mecanum wheels' ik From 69b981463c57608169b902c8e9e3edb355e504cc Mon Sep 17 00:00:00 2001 From: Antoine Rennuit Date: Mon, 13 Jul 2015 16:14:07 +0200 Subject: [PATCH 20/21] [mecanum_drive] Modified the name of the odom "topic" in "odomTopic" as it turned out rather misleading between the different occurences of the odom name (odom used to correspond to a reference frame, a topic and the short name for the odometry class... --- mecanum_drive_controller/src/mecanum_drive_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 3b08c8b12..00420b9ba 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -423,7 +423,7 @@ void MecanumDriveController::setupRtPublishersMsg(ros::NodeHandle& root_nh, ros: ROS_ASSERT(twist_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble); // Setup odometry msg. - odom_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "odom", 100)); + odom_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "odomTopic", 100)); odom_pub_->msg_.header.frame_id = "odom"; odom_pub_->msg_.child_frame_id = base_frame_id_; odom_pub_->msg_.pose.pose.position.z = 0; From de3b6bb609bf5c3b71b9570665383f02f6a823bb Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sun, 21 Mar 2021 20:09:53 +0000 Subject: [PATCH 21/21] Functional fixup for Noetic --- .../mecanum_drive_controller.h | 41 ++- .../mecanum_drive_controller/odometry.h | 39 ++- .../src/mecanum_drive_controller.cpp | 255 ++++++++---------- mecanum_drive_controller/src/odometry.cpp | 46 ++-- 4 files changed, 184 insertions(+), 197 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h index b58f55894..c340a78c2 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h @@ -1,3 +1,7 @@ +#pragma once + +#include + #include #include #include @@ -12,10 +16,8 @@ namespace mecanum_drive_controller { - // Check file README.md for restrictions and notes. -class MecanumDriveController - : public controller_interface::Controller +class MecanumDriveController : public controller_interface::Controller { public: MecanumDriveController(); @@ -26,9 +28,7 @@ class MecanumDriveController * \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); + 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 @@ -66,7 +66,9 @@ class MecanumDriveController double wz_b_b0_b; ros::Time stamp; - Command() : vx_Ob_b_b0_b(0.0), vy_Ob_b_b0_b(0.0), wz_b_b0_b(0.0), stamp(0.0) {} + Command() : vx_Ob_b_b0_b(0.0), vy_Ob_b_b0_b(0.0), wz_b_b0_b(0.0), stamp(0.0) + { + } }; realtime_tools::RealtimeBuffer command_rt_buffer_; Command command_; @@ -75,8 +77,8 @@ class MecanumDriveController /// Odometry related: Odometry odometry_; - boost::shared_ptr > odom_pub_; - boost::shared_ptr > tf_pub_; + std::shared_ptr > odom_pub_; + std::shared_ptr > tf_pub_; geometry_msgs::TransformStamped odom_frame_; @@ -85,15 +87,15 @@ class MecanumDriveController /// Wheel radius (assuming it's the same for the left and right wheels): bool use_realigned_roller_joints_; - double wheels_k_; // wheels geometric param used in mecanum wheels' ik + double wheels_k_; // wheels geometric param used in mecanum wheels' ik double wheels_radius_; /// Timeout to consider cmd_vel commands old: double cmd_vel_timeout_; /// Frame to use for the robot base: - std::string base_frame_id_; - double base_frame_offset_[PLANAR_POINT_DIM]; + std::string base_frame_id_; + double base_frame_offset_[PLANAR_POINT_DIM]; /// Whether to publish odometry to tf or not: bool enable_odom_tf_; @@ -121,11 +123,8 @@ class MecanumDriveController * \param wheel2_name Name of wheel2 joint * \param wheel3_name Name of wheel3 joint */ - bool setWheelParamsFromUrdf(ros::NodeHandle& root_nh, - const std::string& wheel0_name, - const std::string& wheel1_name, - const std::string& wheel2_name, - const std::string& wheel3_name); + bool setWheelParamsFromUrdf(ros::NodeHandle& root_nh, const std::string& wheel0_name, const std::string& wheel1_name, + const std::string& wheel2_name, const std::string& wheel3_name); /** * \brief Get the radius of a given wheel @@ -133,17 +132,17 @@ class MecanumDriveController * \param wheel_link link of the wheel from which to get the radius * \param[out] wheels_radius radius of the wheel read from the urdf */ - bool getWheelRadius(const boost::shared_ptr model, const boost::shared_ptr& wheel_link, double& wheel_radius); + bool getWheelRadius(const urdf::ModelInterfaceSharedPtr& model, const urdf::LinkConstSharedPtr& wheel_link, + double& wheel_radius); /** * \brief Sets the odometry publishing fields * \param root_nh Root node handle * \param controller_nh Node handle inside the controller namespace */ - void setupRtPublishersMsg(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh); - + void setupRealtimePublishersMsg(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh); }; PLUGINLIB_EXPORT_CLASS(mecanum_drive_controller::MecanumDriveController, controller_interface::ControllerBase) -} // namespace mecanum_drive_controller +} // namespace mecanum_drive_controller diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h index 4e0b2b824..0e1a469b4 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h @@ -11,7 +11,6 @@ namespace mecanum_drive_controller { - namespace bacc = boost::accumulators; /// \brief The Odometry class handles odometry readings @@ -19,7 +18,6 @@ namespace bacc = boost::accumulators; class Odometry { public: - /// Integration function, used to integrate the odometry: typedef boost::function IntegrationFunction; @@ -31,7 +29,7 @@ class Odometry /// \brief Initialize the odometry /// \param time Current time - void init(const ros::Time &time, double base_frame_offset[PLANAR_POINT_DIM]); + void init(const ros::Time& time, double base_frame_offset[PLANAR_POINT_DIM]); /// \brief Updates the odometry class with latest wheels position /// \param wheel0_vel Wheel velocity [rad/s] @@ -40,20 +38,38 @@ class Odometry /// \param wheel3_vel Wheel velocity [rad/s] /// \param time Current time /// \return true if the odometry is actually updated - bool update(double wheel0_vel, double wheel1_vel, double wheel2_vel, double wheel3_vel, const ros::Time &time); + bool update(double wheel0_vel, double wheel1_vel, double wheel2_vel, double wheel3_vel, const ros::Time& time); /// \return position (x component) [m] - double getX() const {return px_b_b0_;} + double getX() const + { + return px_b_b0_; + } /// \return position (y component) [m] - double getY() const {return py_b_b0_;} + double getY() const + { + return py_b_b0_; + } /// \return orientation (z component) [m] - double getRz() const {return rz_b_b0_;} + double getRz() const + { + return rz_b_b0_; + } /// \return body velocity of the base frame (linear x component) [m/s] - double getVx() const {return vx_Ob_b_b0_b_;} + double getVx() const + { + return vx_Ob_b_b0_b_; + } /// \return body velocity of the base frame (linear y component) [m/s] - double getVy() const {return vy_Ob_b_b0_b_;} + double getVy() const + { + return vy_Ob_b_b0_b_; + } /// \return body velocity of the base frame (angular z component) [m/s] - double getWz() const {return wz_b_b0_b_;} + double getWz() const + { + return wz_b_b0_b_; + } /// \brief Sets the wheels parameters: mecanum geometric param and radius /// \param wheels_k Wheels geometric param (used in mecanum wheels' ik) [m] @@ -61,7 +77,6 @@ class Odometry void setWheelsParams(double wheels_k, double wheels_radius); private: - /// Rolling mean accumulator and window: typedef bacc::accumulator_set > RollingMeanAcc; typedef bacc::tag::rolling_window RollingWindow; @@ -94,6 +109,6 @@ class Odometry double wheels_radius_; }; -} // namespace mecanum_drive_controller +} // namespace mecanum_drive_controller #endif /* ODOMETRY_H_ */ diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 00420b9ba..98fecd609 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -1,33 +1,32 @@ #include - #include - -#include - #include -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -static bool isCylinderOrSphere(const boost::shared_ptr& link) +static bool isCylinderOrSphere(const urdf::LinkConstSharedPtr& link) { - if(!link) + if (!link) { ROS_ERROR("Link == NULL."); return false; } - if(!link->collision) + if (!link->collision) { - ROS_ERROR_STREAM("Link " << link->name << " does not have collision description. Add collision description for link to urdf."); + ROS_ERROR_STREAM("Link " << link->name + << " does not have collision description. Add collision description for link to urdf."); return false; } - if(!link->collision->geometry) + if (!link->collision->geometry) { - ROS_ERROR_STREAM("Link " << link->name << " does not have collision geometry description. Add collision geometry description for link to urdf."); + ROS_ERROR_STREAM("Link " << link->name + << " does not have collision geometry description. Add collision geometry description for " + "link to urdf."); return false; } - if(link->collision->geometry->type != urdf::Geometry::CYLINDER && link->collision->geometry->type != urdf::Geometry::SPHERE) + if (link->collision->geometry->type != urdf::Geometry::CYLINDER && + link->collision->geometry->type != urdf::Geometry::SPHERE) { ROS_ERROR_STREAM("Link " << link->name << " does not have cylinder nor sphere geometry"); return false; @@ -38,8 +37,6 @@ static bool isCylinderOrSphere(const boost::shared_ptr& link) namespace mecanum_drive_controller { - -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// MecanumDriveController::MecanumDriveController() : command_() , use_realigned_roller_joints_(false) @@ -47,17 +44,14 @@ MecanumDriveController::MecanumDriveController() , wheels_radius_(0.0) , cmd_vel_timeout_(0.5) , base_frame_id_("base_link") - , base_frame_offset_{0.0, 0.0, 0.0} + , base_frame_offset_{ 0.0, 0.0, 0.0 } , enable_odom_tf_(true) , wheel_joints_size_(0) { } - -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -bool MecanumDriveController::init(hardware_interface::VelocityJointInterface* hw, - ros::NodeHandle& root_nh, - ros::NodeHandle &controller_nh) +bool MecanumDriveController::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("/"); @@ -65,7 +59,8 @@ bool MecanumDriveController::init(hardware_interface::VelocityJointInterface* hw // Option use_realigned_roller_joints controller_nh.param("use_realigned_roller_joints", use_realigned_roller_joints_, use_realigned_roller_joints_); - ROS_INFO_STREAM_NAMED(name_, "Use the roller's radius rather than the wheel's: " << (use_realigned_roller_joints_ ? "true" : "false") << "."); + ROS_INFO_STREAM_NAMED(name_, "Use the roller's radius rather than the wheel's: " + << (use_realigned_roller_joints_ ? "true" : "false") << "."); // Get joint names from the parameter server std::string wheel0_name; @@ -87,26 +82,26 @@ bool MecanumDriveController::init(hardware_interface::VelocityJointInterface* hw // Odometry related: double publish_rate; controller_nh.param("publish_rate", publish_rate, 50.0); - ROS_INFO_STREAM_NAMED(name_, "Controller state published frequency : " - << publish_rate << "Hz."); + ROS_INFO_STREAM_NAMED(name_, "Controller state published frequency : " << publish_rate << "Hz."); publish_period_ = ros::Duration(1.0 / publish_rate); // 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."); + ROS_INFO_STREAM_NAMED(name_, + "Velocity commands will be considered old if they are older than " << cmd_vel_timeout_ << "s."); controller_nh.param("base_frame_id", base_frame_id_, base_frame_id_); ROS_INFO_STREAM_NAMED(name_, "Base frame_id : " << base_frame_id_); // TODO: automatically compute the offset of the base frame with respect to the center frame. - controller_nh.param("base_frame_offset/x" , base_frame_offset_[0], base_frame_offset_[0]); - controller_nh.param("base_frame_offset/y" , base_frame_offset_[1], base_frame_offset_[1]); + controller_nh.param("base_frame_offset/x", base_frame_offset_[0], base_frame_offset_[0]); + controller_nh.param("base_frame_offset/y", base_frame_offset_[1], base_frame_offset_[1]); controller_nh.param("base_frame_offset/theta", base_frame_offset_[2], base_frame_offset_[2]); - ROS_INFO_STREAM_NAMED(name_, "base_frame_offset : " << base_frame_offset_[0] << " " << base_frame_offset_[1] << " " << base_frame_offset_[2]); + ROS_INFO_STREAM_NAMED(name_, "base_frame_offset : " << base_frame_offset_[0] << " " << base_frame_offset_[1] << " " + << base_frame_offset_[2]); controller_nh.param("enable_odom_tf", enable_odom_tf_, enable_odom_tf_); - ROS_INFO_STREAM_NAMED(name_, "Publishing to tf : " << (enable_odom_tf_?"enabled":"disabled")); + ROS_INFO_STREAM_NAMED(name_, "Publishing to tf : " << (enable_odom_tf_ ? "enabled" : "disabled")); // Get the joint objects to use in the realtime loop wheel0_jointHandle_ = hw->getHandle(wheel0_name); // throws on failure @@ -118,15 +113,13 @@ bool MecanumDriveController::init(hardware_interface::VelocityJointInterface* hw if (!setWheelParamsFromUrdf(root_nh, wheel0_name, wheel1_name, wheel2_name, wheel3_name)) return false; - setupRtPublishersMsg(root_nh, controller_nh); + setupRealtimePublishersMsg(root_nh, controller_nh); command_sub_ = controller_nh.subscribe("cmd_vel", 1, &MecanumDriveController::commandCb, this); return true; } - -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void MecanumDriveController::update(const ros::Time& time, const ros::Duration& period) { // FORWARD KINEMATICS (odometry). @@ -142,7 +135,7 @@ void MecanumDriveController::update(const ros::Time& time, const ros::Duration& odometry_.update(wheel0_vel, wheel1_vel, wheel2_vel, wheel3_vel, time); // Publish odometry message - if(last_state_publish_time_ + publish_period_ < time) + if (last_state_publish_time_ + publish_period_ < time) { last_state_publish_time_ += publish_period_; @@ -150,14 +143,14 @@ void MecanumDriveController::update(const ros::Time& time, const ros::Duration& const geometry_msgs::Quaternion orientation(tf::createQuaternionMsgFromYaw(odometry_.getRz())); // Populate odom message and publish - if(odom_pub_->trylock()) + 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_.getVx(); - odom_pub_->msg_.twist.twist.linear.y = odometry_.getVy(); + odom_pub_->msg_.twist.twist.linear.x = odometry_.getVx(); + odom_pub_->msg_.twist.twist.linear.y = odometry_.getVy(); odom_pub_->msg_.twist.twist.angular.z = odometry_.getWz(); odom_pub_->unlockAndPublish(); @@ -186,18 +179,18 @@ void MecanumDriveController::update(const ros::Time& time, const ros::Duration& { curr_cmd.vx_Ob_b_b0_b = 0.0; curr_cmd.vy_Ob_b_b0_b = 0.0; - curr_cmd.wz_b_b0_b = 0.0; + curr_cmd.wz_b_b0_b = 0.0; } // Compute wheels velocities (this is the actual ik): // NOTE: the input desired twist (from topic /cmd_vel) is a body twist. - tf::Matrix3x3 R_b_c = tf::Matrix3x3(tf::createQuaternionFromYaw(base_frame_offset_[2])); - tf::Vector3 v_Ob_b_b0_c = R_b_c * tf::Vector3(curr_cmd.vx_Ob_b_b0_b, curr_cmd.vy_Ob_b_b0_b, 0.0); - tf::Vector3 Ob_c = tf::Vector3(base_frame_offset_[0], base_frame_offset_[1], 0.0); + tf::Matrix3x3 R_b_c = tf::Matrix3x3(tf::createQuaternionFromYaw(base_frame_offset_[2])); + tf::Vector3 v_Ob_b_b0_c = R_b_c * tf::Vector3(curr_cmd.vx_Ob_b_b0_b, curr_cmd.vy_Ob_b_b0_b, 0.0); + tf::Vector3 Ob_c = tf::Vector3(base_frame_offset_[0], base_frame_offset_[1], 0.0); double vx_Oc_c_c0_c_ = v_Ob_b_b0_c.x() + Ob_c.y() * curr_cmd.wz_b_b0_b; double vy_Oc_c_c0_c_ = v_Ob_b_b0_c.y() - Ob_c.x() * curr_cmd.wz_b_b0_b; - double wz_c_c0_c_ = curr_cmd.wz_b_b0_b; + double wz_c_c0_c_ = curr_cmd.wz_b_b0_b; double w0_vel = 1.0 / wheels_radius_ * (vx_Oc_c_c0_c_ - vy_Oc_c_c0_c_ - wheels_k_ * wz_c_c0_c_); double w1_vel = 1.0 / wheels_radius_ * (vx_Oc_c_c0_c_ + vy_Oc_c_c0_c_ - wheels_k_ * wz_c_c0_c_); @@ -211,7 +204,6 @@ void MecanumDriveController::update(const ros::Time& time, const ros::Duration& wheel3_jointHandle_.setCommand(w3_vel); } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void MecanumDriveController::starting(const ros::Time& time) { brake(); @@ -222,13 +214,11 @@ void MecanumDriveController::starting(const ros::Time& time) odometry_.init(time, base_frame_offset_); } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void MecanumDriveController::stopping(const ros::Time& time) { brake(); } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void MecanumDriveController::brake() { wheel0_jointHandle_.setCommand(0.0); @@ -237,91 +227,82 @@ void MecanumDriveController::brake() wheel3_jointHandle_.setCommand(0.0); } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void MecanumDriveController::commandCb(const geometry_msgs::Twist& command) { - if (!isRunning()) - { - ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running."); - return; - } + if (!isRunning()) + { + ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running."); + return; + } - command_.vx_Ob_b_b0_b = command.linear.x; - command_.vy_Ob_b_b0_b = command.linear.y; - command_.wz_b_b0_b = command.angular.z; + command_.vx_Ob_b_b0_b = command.linear.x; + command_.vy_Ob_b_b0_b = command.linear.y; + command_.wz_b_b0_b = command.angular.z; - command_.stamp = ros::Time::now(); - command_rt_buffer_.writeFromNonRT(command_); + command_.stamp = ros::Time::now(); + command_rt_buffer_.writeFromNonRT(command_); - ROS_DEBUG_STREAM_NAMED(name_, - "Added values to command. " - << "vx_Ob_b_b0_b : " << command_.vx_Ob_b_b0_b << ", " - << "vy_Ob_b_b0_b : " << command_.vy_Ob_b_b0_b << ", " - << "wz_b_b0_b : " << command_.wz_b_b0_b << ", " - << "Stamp : " << command_.stamp); + ROS_DEBUG_STREAM_NAMED(name_, "Added values to command. " + << "vx_Ob_b_b0_b : " << command_.vx_Ob_b_b0_b << ", " + << "vy_Ob_b_b0_b : " << command_.vy_Ob_b_b0_b << ", " + << "wz_b_b0_b : " << command_.wz_b_b0_b << ", " + << "Stamp : " << command_.stamp); } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -bool MecanumDriveController::setWheelParamsFromUrdf(ros::NodeHandle& root_nh, - const std::string& wheel0_name, - const std::string& wheel1_name, - const std::string& wheel2_name, - const std::string& wheel3_name) +bool MecanumDriveController::setWheelParamsFromUrdf(ros::NodeHandle& root_nh, const std::string& wheel0_name, + const std::string& wheel1_name, const std::string& wheel2_name, + const std::string& wheel3_name) { // 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)) + std::string robot_model_str = ""; + if (!res || !root_nh.getParam(model_param_name, robot_model_str)) { ROS_ERROR_NAMED(name_, "Robot descripion couldn't be retrieved from param server."); return false; } - boost::shared_ptr model(urdf::parseURDF(robot_model_str)); + urdf::ModelInterfaceSharedPtr model(urdf::parseURDF(robot_model_str)); // Get wheels position and compute parameter k_ (used in mecanum wheels IK). - boost::shared_ptr wheel0_urdfJoint(model->getJoint(wheel0_name)); - if(!wheel0_urdfJoint) + urdf::JointConstSharedPtr wheel0_urdfJoint(model->getJoint(wheel0_name)); + if (!wheel0_urdfJoint) { - ROS_ERROR_STREAM_NAMED(name_, wheel0_name - << " couldn't be retrieved from model description"); + ROS_ERROR_STREAM_NAMED(name_, wheel0_name << " couldn't be retrieved from model description"); return false; } - boost::shared_ptr wheel1_urdfJoint(model->getJoint(wheel1_name)); - if(!wheel1_urdfJoint) + urdf::JointConstSharedPtr wheel1_urdfJoint(model->getJoint(wheel1_name)); + if (!wheel1_urdfJoint) { - ROS_ERROR_STREAM_NAMED(name_, wheel1_name - << " couldn't be retrieved from model description"); + ROS_ERROR_STREAM_NAMED(name_, wheel1_name << " couldn't be retrieved from model description"); return false; } - boost::shared_ptr wheel2_urdfJoint(model->getJoint(wheel2_name)); - if(!wheel2_urdfJoint) + urdf::JointConstSharedPtr wheel2_urdfJoint(model->getJoint(wheel2_name)); + if (!wheel2_urdfJoint) { - ROS_ERROR_STREAM_NAMED(name_, wheel2_name - << " couldn't be retrieved from model description"); + ROS_ERROR_STREAM_NAMED(name_, wheel2_name << " couldn't be retrieved from model description"); return false; } - boost::shared_ptr wheel3_urdfJoint(model->getJoint(wheel3_name)); - if(!wheel3_urdfJoint) + urdf::JointConstSharedPtr wheel3_urdfJoint(model->getJoint(wheel3_name)); + if (!wheel3_urdfJoint) { - ROS_ERROR_STREAM_NAMED(name_, wheel3_name - << " couldn't be retrieved from model description"); + ROS_ERROR_STREAM_NAMED(name_, wheel3_name << " couldn't be retrieved from model description"); return false; } - ROS_INFO_STREAM("wheel0 to origin: " << wheel0_urdfJoint->parent_to_joint_origin_transform.position.x << "," - << wheel0_urdfJoint->parent_to_joint_origin_transform.position.y << ", " - << wheel0_urdfJoint->parent_to_joint_origin_transform.position.z); - ROS_INFO_STREAM("wheel1 to origin: " << wheel1_urdfJoint->parent_to_joint_origin_transform.position.x << "," - << wheel1_urdfJoint->parent_to_joint_origin_transform.position.y << ", " - << wheel1_urdfJoint->parent_to_joint_origin_transform.position.z); - ROS_INFO_STREAM("wheel2 to origin: " << wheel2_urdfJoint->parent_to_joint_origin_transform.position.x << "," - << wheel2_urdfJoint->parent_to_joint_origin_transform.position.y << ", " - << wheel2_urdfJoint->parent_to_joint_origin_transform.position.z); - ROS_INFO_STREAM("wheel3 to origin: " << wheel3_urdfJoint->parent_to_joint_origin_transform.position.x << "," - << wheel3_urdfJoint->parent_to_joint_origin_transform.position.y << ", " - << wheel3_urdfJoint->parent_to_joint_origin_transform.position.z); + ROS_INFO_STREAM("wheel0 to origin: " << wheel0_urdfJoint->parent_to_joint_origin_transform.position.x << "," + << wheel0_urdfJoint->parent_to_joint_origin_transform.position.y << ", " + << wheel0_urdfJoint->parent_to_joint_origin_transform.position.z); + ROS_INFO_STREAM("wheel1 to origin: " << wheel1_urdfJoint->parent_to_joint_origin_transform.position.x << "," + << wheel1_urdfJoint->parent_to_joint_origin_transform.position.y << ", " + << wheel1_urdfJoint->parent_to_joint_origin_transform.position.z); + ROS_INFO_STREAM("wheel2 to origin: " << wheel2_urdfJoint->parent_to_joint_origin_transform.position.x << "," + << wheel2_urdfJoint->parent_to_joint_origin_transform.position.y << ", " + << wheel2_urdfJoint->parent_to_joint_origin_transform.position.z); + ROS_INFO_STREAM("wheel3 to origin: " << wheel3_urdfJoint->parent_to_joint_origin_transform.position.x << "," + << wheel3_urdfJoint->parent_to_joint_origin_transform.position.y << ", " + << wheel3_urdfJoint->parent_to_joint_origin_transform.position.z); double wheel0_x = wheel0_urdfJoint->parent_to_joint_origin_transform.position.x; double wheel0_y = wheel0_urdfJoint->parent_to_joint_origin_transform.position.y; @@ -349,8 +330,7 @@ bool MecanumDriveController::setWheelParamsFromUrdf(ros::NodeHandle& root_nh, return false; } - if (fabs(wheel0_radius - wheel1_radius) > 1e-3 || - fabs(wheel0_radius - wheel2_radius) > 1e-3 || + if (fabs(wheel0_radius - wheel1_radius) > 1e-3 || fabs(wheel0_radius - wheel2_radius) > 1e-3 || fabs(wheel0_radius - wheel3_radius) > 1e-3) { ROS_ERROR_STREAM_NAMED(name_, "Wheels radius are not egual"); @@ -367,30 +347,34 @@ bool MecanumDriveController::setWheelParamsFromUrdf(ros::NodeHandle& root_nh, return true; } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -bool MecanumDriveController::getWheelRadius(const boost::shared_ptr model, const boost::shared_ptr& wheel_link, double& wheel_radius) +bool MecanumDriveController::getWheelRadius(const urdf::ModelInterfaceSharedPtr& model, + const urdf::LinkConstSharedPtr& wheel_link, double& wheel_radius) { - boost::shared_ptr radius_link = wheel_link; + auto radius_link = wheel_link; if (use_realigned_roller_joints_) { - // This mode is used when the mecanum wheels are simulated and we use realigned rollers to mimic mecanum wheels. - const boost::shared_ptr& roller_joint = radius_link->child_joints[0]; - if(!roller_joint) - { - ROS_ERROR_STREAM_NAMED(name_, "No roller joint could be retrieved for wheel : " << wheel_link->name << ". Are you sure mecanum wheels are simulated using realigned rollers?"); - return false; - } - - radius_link = model->getLink(roller_joint->child_link_name); - if(!radius_link) - { - ROS_ERROR_STREAM_NAMED(name_, "No roller link could be retrieved for wheel : " << wheel_link->name << ". Are you sure mecanum wheels are simulated using realigned rollers?"); - return false; - } + // This mode is used when the mecanum wheels are simulated and we use realigned rollers to mimic mecanum wheels. + const auto& roller_joint = radius_link->child_joints[0]; + if (!roller_joint) + { + ROS_ERROR_STREAM_NAMED(name_, "No roller joint could be retrieved for wheel : " + << wheel_link->name + << ". Are you sure mecanum wheels are simulated using realigned rollers?"); + return false; + } + + radius_link = model->getLink(roller_joint->child_link_name); + if (!radius_link) + { + ROS_ERROR_STREAM_NAMED(name_, "No roller link could be retrieved for wheel : " + << wheel_link->name + << ". Are you sure mecanum wheels are simulated using realigned rollers?"); + return false; + } } - if(!isCylinderOrSphere(radius_link)) + if (!isCylinderOrSphere(radius_link)) { ROS_ERROR_STREAM("Wheel link " << radius_link->name << " is NOT modeled as a cylinder!"); return false; @@ -404,8 +388,7 @@ bool MecanumDriveController::getWheelRadius(const boost::shared_ptrmsg_.header.frame_id = "odom"; odom_pub_->msg_.child_frame_id = base_frame_id_; odom_pub_->msg_.pose.pose.position.z = 0; - odom_pub_->msg_.pose.covariance = boost::assign::list_of - (static_cast(pose_cov_list[0])) (0) (0) (0) (0) (0) - (0) (static_cast(pose_cov_list[1])) (0) (0) (0) (0) - (0) (0) (static_cast(pose_cov_list[2])) (0) (0) (0) - (0) (0) (0) (static_cast(pose_cov_list[3])) (0) (0) - (0) (0) (0) (0) (static_cast(pose_cov_list[4])) (0) - (0) (0) (0) (0) (0) (static_cast(pose_cov_list[5])); - odom_pub_->msg_.twist.twist.linear.y = 0; - odom_pub_->msg_.twist.twist.linear.z = 0; + odom_pub_->msg_.pose.covariance = { static_cast(pose_cov_list[0]), 0., 0., 0., 0., 0., 0., + static_cast(pose_cov_list[1]), 0., 0., 0., 0., 0., 0., + static_cast(pose_cov_list[2]), 0., 0., 0., 0., 0., 0., + static_cast(pose_cov_list[3]), 0., 0., 0., 0., 0., 0., + static_cast(pose_cov_list[4]), 0., 0., 0., 0., 0., 0., + static_cast(pose_cov_list[5]) }; + odom_pub_->msg_.twist.twist.linear.y = 0; + odom_pub_->msg_.twist.twist.linear.z = 0; odom_pub_->msg_.twist.twist.angular.x = 0; odom_pub_->msg_.twist.twist.angular.y = 0; - odom_pub_->msg_.twist.covariance = boost::assign::list_of - (static_cast(twist_cov_list[0])) (0) (0) (0) (0) (0) - (0) (static_cast(twist_cov_list[1])) (0) (0) (0) (0) - (0) (0) (static_cast(twist_cov_list[2])) (0) (0) (0) - (0) (0) (0) (static_cast(twist_cov_list[3])) (0) (0) - (0) (0) (0) (0) (static_cast(twist_cov_list[4])) (0) - (0) (0) (0) (0) (0) (static_cast(twist_cov_list[5])); + odom_pub_->msg_.twist.covariance = { static_cast(twist_cov_list[0]), 0., 0., 0., 0., 0., 0., + static_cast(twist_cov_list[1]), 0., 0., 0., 0., 0., 0., + static_cast(twist_cov_list[2]), 0., 0., 0., 0., 0., 0., + static_cast(twist_cov_list[3]), 0., 0., 0., 0., 0., 0., + static_cast(twist_cov_list[4]), 0., 0., 0., 0., 0., 0., + static_cast(twist_cov_list[5]) }; // Setup tf msg. tf_pub_.reset(new realtime_tools::RealtimePublisher(root_nh, "/tf", 100)); @@ -454,4 +435,4 @@ void MecanumDriveController::setupRtPublishersMsg(ros::NodeHandle& root_nh, ros: tf_pub_->msg_.transforms[0].header.frame_id = "odom"; } -} // namespace mecanum_drive_controller +} // namespace mecanum_drive_controller diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index 5f6075725..574b20ad7 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -6,26 +6,21 @@ namespace mecanum_drive_controller { - namespace bacc = boost::accumulators; -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// Odometry::Odometry(size_t velocity_rolling_window_size) -: timestamp_(0.0) - -, px_b_b0_(0.0) -, py_b_b0_(0.0) -, rz_b_b0_(0.0) -, vx_Ob_b_b0_b_(0.0) -, vy_Ob_b_b0_b_(0.0) -, wz_b_b0_b_(0.0) - -, wheels_k_(0.0) -, wheels_radius_(0.0) + : timestamp_(0.0) + , px_b_b0_(0.0) + , py_b_b0_(0.0) + , rz_b_b0_(0.0) + , vx_Ob_b_b0_b_(0.0) + , vy_Ob_b_b0_b_(0.0) + , wz_b_b0_b_(0.0) + , wheels_k_(0.0) + , wheels_radius_(0.0) { } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void Odometry::init(const ros::Time& time, double base_frame_offset[PLANAR_POINT_DIM]) { // Reset timestamp: @@ -37,13 +32,12 @@ void Odometry::init(const ros::Time& time, double base_frame_offset[PLANAR_POINT base_frame_offset_[2] = base_frame_offset[2]; } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -bool Odometry::update(double wheel0_vel, double wheel1_vel, double wheel2_vel, double wheel3_vel, const ros::Time &time) +bool Odometry::update(double wheel0_vel, double wheel1_vel, double wheel2_vel, double wheel3_vel, const ros::Time& time) { /// 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 + return false; // Interval too small to integrate with timestamp_ = time; @@ -53,17 +47,17 @@ bool Odometry::update(double wheel0_vel, double wheel1_vel, double wheel2_vel, d /// NOTE: in the diff drive the velocity is filtered out, but we prefer to return it raw and let the user perform /// post-processing at will. We prefer this way of doing as filtering introduces delay (which makes it /// difficult to interpret and compare behavior curves). - double vx_Oc_c_c0_c = 0.25 * wheels_radius_ * ( wheel0_vel + wheel1_vel + wheel2_vel + wheel3_vel); - double vy_Oc_c_c0_c = 0.25 * wheels_radius_ * (-wheel0_vel + wheel1_vel - wheel2_vel + wheel3_vel); - double wz_c_c0_c = 0.25 * wheels_radius_ / wheels_k_ * (-wheel0_vel - wheel1_vel + wheel2_vel + wheel3_vel); + double vx_Oc_c_c0_c = 0.25 * wheels_radius_ * (wheel0_vel + wheel1_vel + wheel2_vel + wheel3_vel); + double vy_Oc_c_c0_c = 0.25 * wheels_radius_ * (-wheel0_vel + wheel1_vel - wheel2_vel + wheel3_vel); + double wz_c_c0_c = 0.25 * wheels_radius_ / wheels_k_ * (-wheel0_vel - wheel1_vel + wheel2_vel + wheel3_vel); - tf::Matrix3x3 R_c_b = tf::Matrix3x3(tf::createQuaternionFromYaw(-base_frame_offset_[2])); - tf::Vector3 v_Oc_c_c0_b = R_c_b * tf::Vector3(vx_Oc_c_c0_c, vy_Oc_c_c0_c, 0.0); - tf::Vector3 Oc_b = R_c_b * tf::Vector3(-base_frame_offset_[0], -base_frame_offset_[1], 0.0); + tf::Matrix3x3 R_c_b = tf::Matrix3x3(tf::createQuaternionFromYaw(-base_frame_offset_[2])); + tf::Vector3 v_Oc_c_c0_b = R_c_b * tf::Vector3(vx_Oc_c_c0_c, vy_Oc_c_c0_c, 0.0); + tf::Vector3 Oc_b = R_c_b * tf::Vector3(-base_frame_offset_[0], -base_frame_offset_[1], 0.0); vx_Ob_b_b0_b_ = v_Oc_c_c0_b.x() + Oc_b.y() * wz_c_c0_c; vy_Ob_b_b0_b_ = v_Oc_c_c0_b.y() - Oc_b.x() * wz_c_c0_c; - wz_b_b0_b_ = wz_c_c0_c; + wz_b_b0_b_ = wz_c_c0_c; /// Integration. /// NOTE: the position is expressed in the odometry frame (frame b0), unlike the twist which is expressed in the body @@ -79,12 +73,10 @@ bool Odometry::update(double wheel0_vel, double wheel1_vel, double wheel2_vel, d return true; } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void Odometry::setWheelsParams(double wheels_k, double wheels_radius) { wheels_k_ = wheels_k; - wheels_radius_ = wheels_radius; } -} // namespace mecanum_drive_controller +} // namespace mecanum_drive_controller