From b036e1866e5b6857d6714bbab18a4a3402000d63 Mon Sep 17 00:00:00 2001 From: Gabriel Urbain Date: Thu, 9 Jan 2020 16:10:26 +0100 Subject: [PATCH 1/6] Add four wheels swerve drive to ROS controllers --- swerve_controller/CMakeLists.txt | 64 +++ .../include/swerve_controller/odometry.h | 203 ++++++++ .../include/swerve_controller/speed_limiter.h | 134 +++++ .../swerve_controller/swerve_controller.h | 217 ++++++++ swerve_controller/package.xml | 37 ++ swerve_controller/src/odometry.cpp | 153 ++++++ swerve_controller/src/speed_limiter.cpp | 132 +++++ swerve_controller/src/swerve_controller.cpp | 449 ++++++++++++++++ .../swerve_controller_plugin.xml | 9 + swerve_controller/test/bin/joy_test | 110 ++++ swerve_controller/test/bin/pub_cmd | 84 +++ swerve_controller/test/bin/tf_ground_truth | 96 ++++ swerve_controller/test/config/swervebot.rviz | 262 ++++++++++ .../test/config/swervebot_control.yaml | 39 ++ .../test/config/swervebot_plotjuggler.xml | 128 +++++ .../include/swerve_controller/swervebot.h | 210 ++++++++ .../test/launch/swervebot.launch | 76 +++ .../test/launch/swervebot_control.launch | 40 ++ .../test/launch/swervebot_plot.launch | 6 + .../test/launch/swervebot_rviz.launch | 35 ++ swerve_controller/test/src/swervebot.cpp | 478 ++++++++++++++++++ swerve_controller/test/swervebot.test | 13 + swerve_controller/test/urdf/materials.xacro | 36 ++ swerve_controller/test/urdf/swervebot.gazebo | 53 ++ swerve_controller/test/urdf/swervebot.xacro | 404 +++++++++++++++ .../test/worlds/swervebot_normal.world | 24 + 26 files changed, 3492 insertions(+) create mode 100644 swerve_controller/CMakeLists.txt create mode 100644 swerve_controller/include/swerve_controller/odometry.h create mode 100644 swerve_controller/include/swerve_controller/speed_limiter.h create mode 100644 swerve_controller/include/swerve_controller/swerve_controller.h create mode 100644 swerve_controller/package.xml create mode 100644 swerve_controller/src/odometry.cpp create mode 100644 swerve_controller/src/speed_limiter.cpp create mode 100644 swerve_controller/src/swerve_controller.cpp create mode 100644 swerve_controller/swerve_controller_plugin.xml create mode 100755 swerve_controller/test/bin/joy_test create mode 100755 swerve_controller/test/bin/pub_cmd create mode 100755 swerve_controller/test/bin/tf_ground_truth create mode 100644 swerve_controller/test/config/swervebot.rviz create mode 100644 swerve_controller/test/config/swervebot_control.yaml create mode 100644 swerve_controller/test/config/swervebot_plotjuggler.xml create mode 100644 swerve_controller/test/include/swerve_controller/swervebot.h create mode 100644 swerve_controller/test/launch/swervebot.launch create mode 100644 swerve_controller/test/launch/swervebot_control.launch create mode 100644 swerve_controller/test/launch/swervebot_plot.launch create mode 100644 swerve_controller/test/launch/swervebot_rviz.launch create mode 100644 swerve_controller/test/src/swervebot.cpp create mode 100644 swerve_controller/test/swervebot.test create mode 100644 swerve_controller/test/urdf/materials.xacro create mode 100644 swerve_controller/test/urdf/swervebot.gazebo create mode 100644 swerve_controller/test/urdf/swervebot.xacro create mode 100644 swerve_controller/test/worlds/swervebot_normal.world diff --git a/swerve_controller/CMakeLists.txt b/swerve_controller/CMakeLists.txt new file mode 100644 index 000000000..5d4241648 --- /dev/null +++ b/swerve_controller/CMakeLists.txt @@ -0,0 +1,64 @@ +cmake_minimum_required(VERSION 2.8.3) +project(swerve_controller) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +# Package dependencies +set(${PROJECT_NAME}_CATKIN_DEPS + controller_interface + joint_state_controller + nav_msgs + realtime_tools + robot_state_publisher + roslint + realtime_tools + tf + urdf_geometry_parser) +list(SORT ${PROJECT_NAME}_CATKIN_DEPS) +find_package(catkin REQUIRED COMPONENTS ${${PROJECT_NAME}_CATKIN_DEPS}) + +# Create the package +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS ${${PROJECT_NAME}_CATKIN_DEPS} +) + +# Source and headers for build +include_directories(include) +include_directories(${catkin_INCLUDE_DIRS}) +add_library(${PROJECT_NAME} src/odometry.cpp src/speed_limiter.cpp src/swerve_controller.cpp) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + +# Paths for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + ) +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +install(FILES swerve_controller_plugin.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +# Tests for CI +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + find_package(std_srvs REQUIRED) + find_package(controller_manager REQUIRED) + + include_directories(test/include) + include_directories(${catkin_INCLUDE_DIRS}) + + add_rostest_gtest(swerve_controller_test + test/swervebot.test + test/src/swervebot.cpp) + + target_link_libraries(swerve_controller_test ${catkin_LIBRARIES}) +endif() + +# Syntax linter for CI +roslint_cpp() \ No newline at end of file diff --git a/swerve_controller/include/swerve_controller/odometry.h b/swerve_controller/include/swerve_controller/odometry.h new file mode 100644 index 000000000..a7c1ddc68 --- /dev/null +++ b/swerve_controller/include/swerve_controller/odometry.h @@ -0,0 +1,203 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Exobotic + * Copyright (c) 2017, Irstea + * 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 Exobotic 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. + *********************************************************************/ + +#ifndef SWERVE_CONTROLLER_ODOMETRY_H_ +#define SWERVE_CONTROLLER_ODOMETRY_H_ + +#include +#include + +namespace swerve_controller +{ + +/** +* \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 + */ + explicit 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 and steerings position + * \param lf_speed front left wheel vehicle speed [rad/s] + * \param rf_speed front right wheel vehicle speed [rad/s] + * \param lh_speed rear left wheel vehicle speed [rad/s] + * \param rh_speed rear right wheel vehicle speed [rad/s] + * \param lf_steering front left steering position [rad] + * \param rf_steering front right steering position [rad] + * \param lh_steering rear left steering position [rad] + * \param rh_steering rear right steering position [rad] + * \param time Current time + * \return true if the odometry is actually updated + */ + bool update(const double& lf_speed, const double& rf_speed, const double& lh_speed, + const double& rh_speed, const double& lf_steering, const double& rf_steering, + const double& lh_steering, const double& rh_steering, 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 along X on the robot base link frame + * \return linear velocity [m/s] + */ + double getLinearX() const + { + return linear_x_; + } + + /** + * \brief linear velocity getter along Y on the robot base link frame + * \return linear velocity [m/s] + */ + double getLinearY() const + { + return linear_y_; + } + + /** + * \brief angular velocity getter + * \return angular velocity [rad/s] + */ + double getAngular() const + { + return angular_; + } + + /** + * \brief Sets the wheel parameters: radius and separation + * \param steering_track Seperation between left and right steering joints [m] + * \param wheel_radius Wheel radius [m] + * \param wheel_base Wheel base [m] + */ + void setWheelParams(double steering_track, double wheel_radius, double wheel_base); + + /** + * \brief Velocity rolling window size setter + * \param velocity_rolling_window_size Velocity rolling window size + */ + void setVelocityRollingWindowSize(size_t velocity_rolling_window_size); + + private: + /** + * \brief Integrates the velocities (linear on x and y and angular) + * \param linear_x Linear velocity along x of the robot frame [m] (linear displacement, i.e. m/s * dt) computed by encoders + * \param linear_y Linear velocity along y of the robot frame [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 integrateXY(double linear_x, double linear_y, double angular); + + /** + * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta + * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by encoders + * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders + */ + void integrateRungeKutta2(double linear, double angular); + + /** + * \brief Integrates the velocities (linear and angular) using exact method + * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by encoders + * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders + */ + void integrateExact(double linear, double angular); + + /** + * \brief Reset linear and angular accumulators + */ + void resetAccumulators(); + + // Current timestamp: + ros::Time last_update_timestamp_; + + // Current pose: + double x_; // [m] + double y_; // [m] + double heading_; // [rad] + + // Current velocity: + double linear_x_, linear_y_; // [m/s] + double angular_; // [rad/s] + + // Wheel kinematic parameters [m]: + double steering_track_; + double wheel_steering_y_offset_; + double wheel_radius_; + double wheel_base_; +}; + +} // namespace swerve_controller + +#endif // SWERVE_CONTROLLER_ODOMETRY_H_ diff --git a/swerve_controller/include/swerve_controller/speed_limiter.h b/swerve_controller/include/swerve_controller/speed_limiter.h new file mode 100644 index 000000000..d6c88644f --- /dev/null +++ b/swerve_controller/include/swerve_controller/speed_limiter.h @@ -0,0 +1,134 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Exobotic + * 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. + *********************************************************************/ + +#ifndef SWERVE_CONTROLLER_SPEED_LIMITER_H +#define SWERVE_CONTROLLER_SPEED_LIMITER_H + +#include + +namespace swerve_controller +{ + +template +T clamp(T x, T min, T max) +{ + return std::min(std::max(min, x), max); +} + +class SpeedLimiter +{ + public: + /** + * \brief Constructor + * \param [in] has_velocity_limits if true, applies velocity limits + * \param [in] has_acceleration_limits if true, applies acceleration limits + * \param [in] has_jerk_limits if true, applies jerk limits + * \param [in] min_velocity Minimum velocity [m/s], usually <= 0 + * \param [in] max_velocity Maximum velocity [m/s], usually >= 0 + * \param [in] min_acceleration Minimum acceleration [m/s^2], usually <= 0 + * \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0 + * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0 + * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 + */ + SpeedLimiter( + bool has_velocity_limits = false, + bool has_acceleration_limits = false, + bool has_jerk_limits = false, + double min_velocity = 0.0, + double max_velocity = 0.0, + double min_acceleration = 0.0, + double max_acceleration = 0.0, + double min_jerk = 0.0, + double max_jerk = 0.0); + + /** + * \brief Limit the velocity and acceleration + * \param [in, out] v Velocity [m/s] + * \param [in] v0 Previous velocity to v [m/s] + * \param [in] v1 Previous velocity to v0 [m/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit(double& v, double v0, double v1, double dt); + + /** + * \brief Limit the velocity + * \param [in, out] v Velocity [m/s] + * \return Limiting factor (1.0 if none) + */ + double limit_velocity(double& v); + + /** + * \brief Limit the acceleration + * \param [in, out] v Velocity [m/s] + * \param [in] v0 Previous velocity [m/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit_acceleration(double& v, double v0, double dt); + + /** + * \brief Limit the jerk + * \param [in, out] v Velocity [m/s] + * \param [in] v0 Previous velocity to v [m/s] + * \param [in] v1 Previous velocity to v0 [m/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + * \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control + */ + double limit_jerk(double& v, double v0, double v1, double dt); + + public: + // Enable/Disable velocity/acceleration/jerk limits: + bool has_velocity_limits; + bool has_acceleration_limits; + bool has_jerk_limits; + + // Velocity limits: + double min_velocity; + double max_velocity; + + // Acceleration limits: + double min_acceleration; + double max_acceleration; + + // Jerk limits: + double min_jerk; + double max_jerk; +}; + +} // namespace swerve_controller + +#endif // SWERVE_CONTROLLER_SPEED_LIMITER_H diff --git a/swerve_controller/include/swerve_controller/swerve_controller.h b/swerve_controller/include/swerve_controller/swerve_controller.h new file mode 100644 index 000000000..caebc0312 --- /dev/null +++ b/swerve_controller/include/swerve_controller/swerve_controller.h @@ -0,0 +1,217 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Exobotic + * Copyright (c) 2017, Irstea + * 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 Exobotic 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. + *********************************************************************/ + +#ifndef SWERVE_CONTROLLER_SWERVE_CONTROLLER_H +#define SWERVE_CONTROLLER_SWERVE_CONTROLLER_H + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +namespace swerve_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 SwerveController + : public controller_interface::MultiInterfaceController +{ + public: + SwerveController(); + + /** + * \brief Initialize controller + * \param robot_hw Velocity and position 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::RobotHW* robot_hw, + ros::NodeHandle& root_nh, + ros::NodeHandle &controller_nh); + + /** + * \brief Updates controller, i.e. computes the odometry and sets the new velocity commands + * \param time Current time + * \param period Time since the last called to update + */ + void update(const ros::Time& time, const ros::Duration& period); + + /** + * \brief Starts controller + * \param time Current time + */ + void starting(const ros::Time& time); + + /** + * \brief Stops controller + * \param time Current time + */ + void stopping(const ros::Time& /*time*/); + + private: + std::string name_; + + /// Odometry related: + ros::Duration publish_period_; + ros::Time last_state_publish_time_; + bool open_loop_; + + /// Hardware handles: + boost::optional lf_wheel_joint_; + boost::optional rf_wheel_joint_; + boost::optional lh_wheel_joint_; + boost::optional rh_wheel_joint_; + boost::optional lf_steering_joint_; + boost::optional rf_steering_joint_; + boost::optional lh_steering_joint_; + boost::optional rh_steering_joint_; + + /// Velocity command related: + struct CommandTwist + { + ros::Time stamp; + double lin_x; + double lin_y; + double ang; + + CommandTwist() : lin_x(0.0), lin_y(0.0), ang(0.0), stamp(0.0) {} + }; + realtime_tools::RealtimeBuffer command_twist_; + CommandTwist command_struct_twist_; + ros::Subscriber sub_command_; + + /// Odometry related: + std::shared_ptr > odom_pub_; + std::shared_ptr > tf_odom_pub_; + Odometry odometry_; + + /// Wheel separation (or track), distance between left and right wheels + /// (from the midpoint of the wheel width) + double track_; + + /// Distance between a wheel joint (from the midpoint of the wheel width) and the + /// associated steering joint: We consider that the distance is the same for every wheel + double wheel_steering_y_offset_; + + /// Wheel radius (assuming it's the same for the left and right wheels): + double wheel_radius_; + + /// Wheel base (distance between front and rear wheel): + double wheel_base_; + + /// 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_; + + /// Speed limiters: + CommandTwist last1_cmd_; + CommandTwist last0_cmd_; + SpeedLimiter limiter_lin_; + SpeedLimiter limiter_ang_; + + private: + /** + * \brief Update and publish odometry + * \param time Current time + */ + void updateOdometry(const ros::Time &time); + /** + * \brief Compute and publish command + * \param time Current time + * \param period Time since the last called to update + */ + void updateCommand(const ros::Time& time, const ros::Duration& period); + + /** + * \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 Populates this class with the robot physical parameters at init + * \param controller_nh Node handle inside the controller namespace + */ + bool getPhysicalParams(ros::NodeHandle& controller_nh); + + /** + * \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(swerve_controller::SwerveController, + controller_interface::ControllerBase); + +} // namespace swerve_controller + +#endif // SWERVE_CONTROLLER_SWERVE_CONTROLLER_H diff --git a/swerve_controller/package.xml b/swerve_controller/package.xml new file mode 100644 index 000000000..ee783cace --- /dev/null +++ b/swerve_controller/package.xml @@ -0,0 +1,37 @@ + + + swerve_controller + 0.0.0 + A controller for ros_control which can operate swerve drive on a robot with four steered wheels + + Gabriel Urbain + + BSD + + http://62.138.26.28:8888/exobotic_land/fieldbot_ros + + Gabriel Urbain + + catkin + + roslint + roslint + + controller_interface + joint_state_controller + robot_state_publisher + nav_msgs + realtime_tools + tf + urdf_geometry_parser + + rosgraph_msgs + rostest + std_srvs + controller_manager + + + + + + \ No newline at end of file diff --git a/swerve_controller/src/odometry.cpp b/swerve_controller/src/odometry.cpp new file mode 100644 index 000000000..c5cc6597e --- /dev/null +++ b/swerve_controller/src/odometry.cpp @@ -0,0 +1,153 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Exobotic + * Copyright (c) 2017, Irstea + * 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 Irstea 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. + *********************************************************************/ + +#include + +#include + +namespace swerve_controller +{ + + Odometry::Odometry(size_t velocity_rolling_window_size) + : last_update_timestamp_(0.0) + , x_(0.0) + , y_(0.0) + , heading_(0.0) + , linear_x_(0.0) + , linear_y_(0.0) + , angular_(0.0) + , steering_track_(0.0) + , wheel_radius_(0.0) + , wheel_base_(0.0) + { + } + + void Odometry::init(const ros::Time& time) + { + // Reset accumulators and timestamp: + last_update_timestamp_ = time; + } + + bool Odometry::update(const double &lf_speed, const double &rf_speed, + const double &lh_speed, const double &rh_speed, + const double &lf_steering, const double &rf_steering, + const double &lh_steering, const double &rh_steering, + const ros::Time &time) + { + // Compute velocity vectors in X-Y for each wheel + const double lf_b = sin(lf_steering) * lf_speed * wheel_radius_; + const double lf_d = cos(lf_steering) * lf_speed * wheel_radius_; + const double rf_b = sin(rf_steering) * rf_speed * wheel_radius_; + const double rf_c = cos(rf_steering) * rf_speed * wheel_radius_; + const double lh_a = sin(lh_steering) * lh_speed * wheel_radius_; + const double lh_d = cos(lh_steering) * lh_speed * wheel_radius_; + const double rh_a = sin(rh_steering) * rh_speed * wheel_radius_; + const double rh_c = cos(rh_steering) * rh_speed * wheel_radius_; + + // Compute robot velocities components + const double a = (rh_a + lh_a) / 2.0; + const double b = (rf_b + lf_b) / 2.0; + const double c = (rf_c + rh_c) / 2.0; + const double d = (lf_d + lh_d) / 2.0; + + // Average angular speed + const double angular_1 = (b - a) / wheel_base_; + const double angular_2 = (c - d) / steering_track_; + angular_ = (angular_1 + angular_2) / 2.0; + + // Average linear speed + const double linear_x_1 = angular_ * (wheel_base_ / 2.0) + c; + const double linear_x_2 = -angular_ * (wheel_base_ / 2.0) + d; + const double linear_y_1 = angular_ * (steering_track_ / 2.0) + a; + const double linear_y_2 = -angular_ * (steering_track_ / 2.0) + b; + + linear_x_ = (linear_x_1 + linear_x_2) / 2.0; + linear_y_ = (linear_y_1 + linear_y_2) / 2.0; + + // Avoid too small intervals + const double dt = (time - last_update_timestamp_).toSec(); + if (dt < 0.0001) + return false; + + // Integrate odometry + last_update_timestamp_ = time; + integrateXY(linear_x_*dt, linear_y_*dt, angular_*dt); + + return true; + } + + void Odometry::setWheelParams(double steering_track, double wheel_radius, double wheel_base) + { + steering_track_ = steering_track; + wheel_radius_ = wheel_radius; + wheel_base_ = wheel_base; + } + + void Odometry::integrateXY(double linear_x, double linear_y, double angular) + { + const double delta_x = linear_x*cos(heading_) - linear_y*sin(heading_); + const double delta_y = linear_x*sin(heading_) + linear_y*cos(heading_); + + x_ += delta_x; + y_ += delta_y; + heading_ += angular; + } + + 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; + } + + void Odometry::integrateExact(double linear, double angular) + { + if (fabs(angular) < 1e-6) + integrateRungeKutta2(linear, angular); + else + { + /// Exact integration (should solve problems when angular is zero): + const double heading_old = heading_; + const double r = linear/angular; + heading_ += angular; + x_ += r * (sin(heading_) - sin(heading_old)); + y_ += -r * (cos(heading_) - cos(heading_old)); + } + } + +} // namespace swerve_controller diff --git a/swerve_controller/src/speed_limiter.cpp b/swerve_controller/src/speed_limiter.cpp new file mode 100644 index 000000000..f0fba4455 --- /dev/null +++ b/swerve_controller/src/speed_limiter.cpp @@ -0,0 +1,132 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Exobotic + * 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 + +namespace swerve_controller +{ + + SpeedLimiter::SpeedLimiter( + bool has_velocity_limits, + bool has_acceleration_limits, + bool has_jerk_limits, + double min_velocity, + double max_velocity, + double min_acceleration, + double max_acceleration, + double min_jerk, + double max_jerk + ) + : has_velocity_limits(has_velocity_limits) + , has_acceleration_limits(has_acceleration_limits) + , has_jerk_limits(has_jerk_limits) + , min_velocity(min_velocity) + , max_velocity(max_velocity) + , min_acceleration(min_acceleration) + , max_acceleration(max_acceleration) + , min_jerk(min_jerk) + , max_jerk(max_jerk) + { + } + + double SpeedLimiter::limit(double& v, double v0, double v1, double dt) + { + const double tmp = v; + + limit_jerk(v, v0, v1, dt); + limit_acceleration(v, v0, dt); + limit_velocity(v); + + return tmp != 0.0 ? v / tmp : 1.0; + } + + double SpeedLimiter::limit_velocity(double& v) + { + const double tmp = v; + + if (has_velocity_limits) + { + v = clamp(v, min_velocity, max_velocity); + } + + return tmp != 0.0 ? v / tmp : 1.0; + } + + double SpeedLimiter::limit_acceleration(double& v, double v0, double dt) + { + const double tmp = v; + + if (has_acceleration_limits) + { + const double dv_min = min_acceleration * dt; + const double dv_max = max_acceleration * dt; + + const double dv = clamp(v - v0, dv_min, dv_max); + + v = v0 + dv; + } + + return tmp != 0.0 ? v / tmp : 1.0; + } + + double SpeedLimiter::limit_jerk(double& v, double v0, double v1, double dt) + { + const double tmp = v; + + if (has_jerk_limits) + { + const double dv = v - v0; + const double dv0 = v0 - v1; + + const double dt2 = 2. * dt * dt; + + const double da_min = min_jerk * dt2; + const double da_max = max_jerk * dt2; + + const double da = clamp(dv - dv0, da_min, da_max); + + v = v0 + dv0 + da; + } + + return tmp != 0.0 ? v / tmp : 1.0; + } + +} // namespace swerve_controller diff --git a/swerve_controller/src/swerve_controller.cpp b/swerve_controller/src/swerve_controller.cpp new file mode 100644 index 000000000..40db3b0a8 --- /dev/null +++ b/swerve_controller/src/swerve_controller.cpp @@ -0,0 +1,449 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Exobotic + * Copyright (c) 2017, Irstea + * 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 Exobotic 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. + *********************************************************************/ + +#include +#include + +namespace swerve_controller +{ + + SwerveController::SwerveController() + : command_struct_twist_() + , track_(0.0) + , wheel_steering_y_offset_(0.0) + , wheel_radius_(0.0) + , wheel_base_(0.0) + , cmd_vel_timeout_(0.5) + , base_frame_id_("base_link") + , enable_odom_tf_(true) + { + } + + bool SwerveController::init(hardware_interface::RobotHW *robot_hw, + ros::NodeHandle& root_nh, + ros::NodeHandle &controller_nh) + { + // Get namespace + std::string complete_ns = controller_nh.getNamespace(); + std::size_t id = complete_ns.find_last_of("/"); + name_ = complete_ns.substr(id + 1); + + // Get wheel joint names from the parameter server + std::string lf_wheel_name, rf_wheel_name, lh_wheel_name, rh_wheel_name; + if (!controller_nh.param("lf_wheel", lf_wheel_name, lf_wheel_name) || + !controller_nh.param("rf_wheel", rf_wheel_name, rf_wheel_name) || + !controller_nh.param("lh_wheel", lh_wheel_name, lh_wheel_name) || + !controller_nh.param("rh_wheel", rh_wheel_name, rh_wheel_name)) + { + ROS_ERROR_STREAM_NAMED(name_, + "Couldn't retrieve wheel joint param !"); + return false; + } + + // Get steering joint names from the parameter server + std::string lf_steering_name, rf_steering_name, lh_steering_name, rh_steering_name; + if (!controller_nh.param("lf_steering", lf_steering_name, lf_steering_name) || + !controller_nh.param("rf_steering", rf_steering_name, rf_steering_name) || + !controller_nh.param("lh_steering", lh_steering_name, lh_steering_name) || + !controller_nh.param("rh_steering", rh_steering_name, rh_steering_name)) + { + ROS_ERROR_STREAM_NAMED(name_, + "Couldn't retrieve steering joint param !"); + return false; + } + + // Get publisher related from the parameter server + 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); + + + // Get twist related from the parameter server + 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")); + + // Get velocity and acceleration limits from the parameter server + 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); + + // Get robot physical parameters from URDF or parameter server + bool lookup_track = !controller_nh.getParam("track", track_); + bool lookup_wheel_radius = !controller_nh.getParam("wheel_radius", wheel_radius_); + bool lookup_wheel_base = !controller_nh.getParam("wheel_base", wheel_base_); + + urdf_geometry_parser::UrdfGeometryParser uvk(root_nh, base_frame_id_); + if (lookup_track) + if (!uvk.getDistanceBetweenJoints(lf_wheel_name, rf_wheel_name, track_)) + return false; + else + controller_nh.setParam("track", track_); + + if (!uvk.getDistanceBetweenJoints(lf_steering_name, lf_wheel_name, + wheel_steering_y_offset_)) + return false; + else + controller_nh.setParam("wheel_steering_y_offset", wheel_steering_y_offset_); + + if (lookup_wheel_radius) + if (!uvk.getJointRadius(lf_wheel_name, wheel_radius_)) + return false; + else + controller_nh.setParam("wheel_radius", wheel_radius_); + + if (lookup_wheel_base) + if (!uvk.getDistanceBetweenJoints(lf_wheel_name, lh_wheel_name, + wheel_base_)) + return false; + else + controller_nh.setParam("wheel_base", wheel_base_); + + // Set physical parameters in odometry + odometry_.setWheelParams(track_-2*wheel_steering_y_offset_, wheel_radius_, wheel_base_); + ROS_INFO_STREAM_NAMED(name_, "Odometry params : track " << track_ << ", wheel radius " + << wheel_radius_ << ", wheel base " << wheel_base_ + << ", wheel steering offset " << wheel_steering_y_offset_); + setOdomPubFields(root_nh, controller_nh); + + + // Get hardware interface + hardware_interface::VelocityJointInterface *const vel_joint_hw = + robot_hw->get(); + hardware_interface::PositionJointInterface *const pos_joint_hw = + robot_hw->get(); + + // Get the wheel joint object to use in the realtime loop + ROS_INFO_STREAM_NAMED(name_, "Adding LF wheel with joint name: " << lf_wheel_name + << ", RF wheel with joint name: " << rf_wheel_name + << ", LH wheel with joint name: " << lh_wheel_name + << ", RH wheel with joint name: " << rh_wheel_name); + lf_wheel_joint_ = vel_joint_hw->getHandle(lf_wheel_name); // throws on failure + rf_wheel_joint_ = vel_joint_hw->getHandle(rf_wheel_name); // throws on failure + lh_wheel_joint_ = vel_joint_hw->getHandle(lh_wheel_name); // throws on failure + rh_wheel_joint_ = vel_joint_hw->getHandle(rh_wheel_name); // throws on failure + + // Get the steering joint object to use in the realtime loop + ROS_INFO_STREAM_NAMED(name_, "Adding LF steering with joint name: " << lf_steering_name + << ", RF steering with joint name: " << rf_steering_name + << ", LH steering with joint name: " << lh_steering_name + << ", RH steering with joint name: " << rh_steering_name); + lf_steering_joint_ = pos_joint_hw->getHandle(lf_steering_name); // throws on failure + rf_steering_joint_ = pos_joint_hw->getHandle(rf_steering_name); // throws on failure + lh_steering_joint_ = pos_joint_hw->getHandle(lh_steering_name); // throws on failure + rh_steering_joint_ = pos_joint_hw->getHandle(rh_steering_name); // throws on failure + + // Subscribe to Twist messages + sub_command_ = controller_nh.subscribe("cmd_vel", 1, + &SwerveController::cmdVelCallback, this); + return true; + } + + void SwerveController::update(const ros::Time& time, const ros::Duration& period) + { + updateOdometry(time); + updateCommand(time, period); + } + + void SwerveController::starting(const ros::Time& time) + { + brake(); + + // Register starting time used to keep fixed rate + last_state_publish_time_ = time; + + odometry_.init(time); + } + + void SwerveController::stopping(const ros::Time& time) + { + brake(); + } + + void SwerveController::updateOdometry(const ros::Time& time) + { + // Get current position and velocities + const double lf_speed = lf_wheel_joint_->getVelocity(); + const double rf_speed = rf_wheel_joint_->getVelocity(); + const double lh_speed = lh_wheel_joint_->getVelocity(); + const double rh_speed = rh_wheel_joint_->getVelocity(); + if (std::isnan(lf_speed) || std::isnan(rf_speed) + || std::isnan(lh_speed) || std::isnan(rh_speed)) + return; + + const double lf_steering = lf_steering_joint_->getPosition(); + const double rf_steering = rf_steering_joint_->getPosition(); + const double lh_steering = lh_steering_joint_->getPosition(); + const double rh_steering = rh_steering_joint_->getPosition(); + if (std::isnan(lf_steering) || std::isnan(rf_steering) + || std::isnan(lh_steering) || std::isnan(rh_steering)) + return; + + // Estimate linear and angular velocity using joint information + odometry_.update(lf_speed, rf_speed, lh_speed, rh_speed, + lf_steering, rf_steering, lh_steering, rh_steering, 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_.getLinearX(); + odom_pub_->msg_.twist.twist.linear.y = odometry_.getLinearY(); + 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(); + } + } + } + + void SwerveController::updateCommand(const ros::Time& time, const ros::Duration& period) + { + // Retreive current velocity command and time step + CommandTwist curr_cmd = *(command_twist_.readFromRT()); + const double dt = (time - curr_cmd.stamp).toSec(); + + // Brake if cmd_vel has timeout + if (dt > cmd_vel_timeout_) + { + brake(); + return; + } + + // Create velocities and position variables + const double cmd_dt(period.toSec()); + const double steering_track = track_- 2 * wheel_steering_y_offset_; + double lf_speed = 0, rf_speed = 0, lh_speed = 0, rh_speed = 0; + double lf_steering = 0, rf_steering = 0, lh_steering = 0, rh_steering = 0; + + // Limit velocities and accelerations: + limiter_lin_.limit(curr_cmd.lin_x, last0_cmd_.lin_x, last1_cmd_.lin_x, cmd_dt); + limiter_lin_.limit(curr_cmd.lin_y, last0_cmd_.lin_y, last1_cmd_.lin_y, cmd_dt); + limiter_ang_.limit(curr_cmd.ang, last0_cmd_.ang, last1_cmd_.ang, cmd_dt); + last1_cmd_ = last0_cmd_; + last0_cmd_ = curr_cmd; + + // Compute wheels velocities and steering angles + if ((fabs(curr_cmd.lin_x) > 0.001) || (fabs(curr_cmd.lin_y) > 0.001) || + (fabs(curr_cmd.ang) > 0.001)) + { + double a = curr_cmd.lin_y - curr_cmd.ang * wheel_base_ / 2; + double b = curr_cmd.lin_y + curr_cmd.ang * wheel_base_ / 2; + double c = curr_cmd.lin_x - curr_cmd.ang * steering_track / 2; + double d = curr_cmd.lin_x + curr_cmd.ang * steering_track / 2; + + lf_speed = sqrt(pow(b, 2) + pow(c, 2)) / wheel_radius_; + rf_speed = sqrt(pow(b, 2) + pow(d, 2)) / wheel_radius_; + lh_speed = sqrt(pow(a, 2) + pow(c, 2)) / wheel_radius_; + rh_speed = sqrt(pow(a, 2) + pow(d, 2)) / wheel_radius_; + + lf_steering = atan2(b, c); + rf_steering = atan2(b, d); + lh_steering = atan2(a, c); + rh_steering = atan2(a, d); + } + + // Set wheels velocities + if (lf_wheel_joint_ && rf_wheel_joint_ && lh_wheel_joint_ && rh_wheel_joint_) + { + lf_wheel_joint_->setCommand(lf_speed); + rf_wheel_joint_->setCommand(rf_speed); + lh_wheel_joint_->setCommand(lh_speed); + rh_wheel_joint_->setCommand(rh_speed); + } + + // Set wheels steering angles + if (lf_wheel_joint_ && rf_wheel_joint_ && lh_wheel_joint_ && rh_wheel_joint_) + { + lf_steering_joint_->setCommand(lf_steering); + rf_steering_joint_->setCommand(rf_steering); + lh_steering_joint_->setCommand(lh_steering); + rh_steering_joint_->setCommand(rh_steering); + } + } + + void SwerveController::brake() + { + // Set wheels velocities + if (lf_wheel_joint_ && rf_wheel_joint_ && lh_wheel_joint_ && rh_wheel_joint_) + { + lf_wheel_joint_->setCommand(0.0); + rf_wheel_joint_->setCommand(0.0); + lh_wheel_joint_->setCommand(0.0); + rh_wheel_joint_->setCommand(0.0); + } + + // Set wheels steering angles + if (lf_wheel_joint_ && rf_wheel_joint_ && lh_wheel_joint_ && rh_wheel_joint_) + { + lf_steering_joint_->setCommand(0.0); + rf_steering_joint_->setCommand(0.0); + lh_steering_joint_->setCommand(0.0); + rh_steering_joint_->setCommand(0.0); + } + } + + void SwerveController::cmdVelCallback(const geometry_msgs::Twist& command) + { + if (isRunning()) + { + if (std::isnan(command.angular.z) || std::isnan(command.linear.x)) + { + ROS_WARN("Received NaN in geometry_msgs::Twist. Ignoring command."); + return; + } + command_struct_twist_.ang = command.angular.z; + command_struct_twist_.lin_x = command.linear.x; + command_struct_twist_.lin_y = command.linear.y; + command_struct_twist_.stamp = ros::Time::now(); + command_twist_.writeFromNonRT(command_struct_twist_); + ROS_DEBUG_STREAM_NAMED(name_, "Added values to command. " << "Ang: " << + command_struct_twist_.ang << ", Lin x: " << + command_struct_twist_.lin_x << ", Lin y: " << + command_struct_twist_.lin_y << ", Stamp: " << + command_struct_twist_.stamp); + } + else + { + ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running."); + } + } + + void SwerveController::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 = + { 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 = + { 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 swerve_controller diff --git a/swerve_controller/swerve_controller_plugin.xml b/swerve_controller/swerve_controller_plugin.xml new file mode 100644 index 000000000..3e582dfc3 --- /dev/null +++ b/swerve_controller/swerve_controller_plugin.xml @@ -0,0 +1,9 @@ + + + + + The SwerveController tracks Twist messages with X and Y velocities and Z rotation. It expects 4 VelocityJointInterface type and 4 PositionJointInterface type of hardware interfaces. + + + + diff --git a/swerve_controller/test/bin/joy_test b/swerve_controller/test/bin/joy_test new file mode 100755 index 000000000..49b864961 --- /dev/null +++ b/swerve_controller/test/bin/joy_test @@ -0,0 +1,110 @@ +#! /usr/bin/env python + +########################################################################## +# Software License Agreement (BSD License) +# +# Copyright (c) 2020, Exobotic +# 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 Exobotic 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. +########################################################################## + +import rospy as ros +import threading +import time + +from std_msgs.msg import Float64 +from geometry_msgs.msg import Twist, Vector3 +from sensor_msgs.msg import Joy + + +class SwerveJoystick(): + + def __init__(self, rate=100, vx_i=0.0, vy_i=0.0, omega_i=0.0): + + # Swerve controller + self.vx = vx_i + self.vy = vy_i + self.omega = omega_i + self.v_lin_max = 1.5 # m/s + self.v_rot_max = 2 # rad/s + + # ROS messages + self.rate = ros.Rate(rate) + self.joy_sub_name = "joy" + self.twist_pub_name = "/swervebot/swerve_controller/cmd_vel" + self.twist_pub = None + self.twist_pub_data = None + self.twist_pub_thread_handle = None + + def __del__(self): + + if self.twist_pub_thread_handle is not None: + self.twist_pub_thread_handle.join() + + def init_ros(self): + + # Twist publishers + self.twist_pub = ros.Publisher(self.twist_pub_name, Twist, queue_size=1) + self.twist_pub_data = Twist() + + # Joystick subscriber + self.joy_sub = ros.Subscriber(self.joy_sub_name, Joy, self.joy_thread) + + # Publishing thread + self.run() + + def run(self): + + self.twist_pub_thread_handle = threading.Thread(target=self.pub_thread) + self.twist_pub_thread_handle.start() + + def pub_thread(self): + + while not ros.is_shutdown(): + + self.twist_pub.publish(self.twist_pub_data) + self.rate.sleep() + + def joy_thread(self, msg): + + self.vx = msg.axes[1] * self.v_lin_max + self.vy = msg.axes[0] * self.v_lin_max + self.omega = msg.axes[3] * self.v_rot_max + + self.twist_pub_data = Twist(linear=Vector3(x=self.vx, y=self.vy), angular=Vector3(z=self.omega)) + + +if __name__ == "__main__": + + ros.init_node("test_swerve_control") + swervebot = SwerveJoystick() + swervebot.init_ros() + + while not ros.is_shutdown(): + time.sleep(1) \ No newline at end of file diff --git a/swerve_controller/test/bin/pub_cmd b/swerve_controller/test/bin/pub_cmd new file mode 100755 index 000000000..ccfc3d7ea --- /dev/null +++ b/swerve_controller/test/bin/pub_cmd @@ -0,0 +1,84 @@ +#! /usr/bin/env python + +########################################################################## +# Software License Agreement (BSD License) +# +# Copyright (c) 2020, Exobotic +# 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 Exobotic 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. +########################################################################## + +import copy +from geometry_msgs.msg import Twist +from nav_msgs.msg import Odometry +import rospy as ros +import time + +def odomCallback(odom): + global last_x + last_x = odom.pose.pose.position.x + +if __name__ == "__main__": + + global last_x + last_x = 0 + ros.init_node("test_pub") + pub = ros.Publisher("/swervebot/swerve_controller/cmd_vel", Twist, queue_size=1) + sub = ros.Subscriber("/swervebot/swerve_controller/odom", Odometry, odomCallback) + + + cmd_vel = Twist() + cmd_vel.linear.x = 0.0 + cmd_vel.linear.y = 0.0 + cmd_vel.angular.z = 0.0 + + prev_time = time.time() + while (time.time() - prev_time < 0.1) and not ros.is_shutdown(): + cmd_vel.linear.x = 0.5 + pub.publish(cmd_vel) + + + init_x = copy.copy(last_x) + cmd_vel.linear.x = 0.5 + prev_time = time.time() + while (time.time() - prev_time < 10) and not ros.is_shutdown(): + pub.publish(cmd_vel) + + cmd_vel.linear.x = 0.0 + prev_time = time.time() + while (time.time() - prev_time < 0.1) and not ros.is_shutdown(): + cmd_vel.linear.x = 0.5 + pub.publish(cmd_vel) + + final_x = copy.copy(last_x) + + print "DIFF: " + str(final_x - init_x) + print final_x, init_x + + diff --git a/swerve_controller/test/bin/tf_ground_truth b/swerve_controller/test/bin/tf_ground_truth new file mode 100755 index 000000000..a1d352be9 --- /dev/null +++ b/swerve_controller/test/bin/tf_ground_truth @@ -0,0 +1,96 @@ +#! /usr/bin/env python + +########################################################################## +# Software License Agreement (BSD License) +# +# Copyright (c) 2020, Exobotic +# 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 Exobotic 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. +########################################################################## + +import rospy as ros +import numpy as np +import sys +import tf +from nav_msgs.msg import Odometry + +class Link2TF(): + + def __init__(self): + + ros.init_node("tf_world_publisher") + + self.rate = ros.Rate(100) + self.tf_pub = tf.TransformBroadcaster() + self.tf_orig = "odom" + self.tf_dest = "world" + self.world_sub = ros.Subscriber("/swervebot/ground_truth_pose", Odometry, self.update_world) + self.odom_sub = ros.Subscriber("/swervebot/swerve_controller/odom", Odometry, self.update_odom) + + self.world_pose = np.array([0, 0, 0]) + self.odom_pose = np.array([0, 0, 0]) + self.world_ori = [0, 0, 0, 0] + self.odom_ori = [0, 0, 0, 0] + + def run(self): + + while not ros.is_shutdown(): + + self.tf_pub.sendTransform(self.world_pose - self.odom_pose, + tf.transformations.quaternion_multiply(self.world_ori, self.odom_ori), + ros.Time.now(), self.tf_orig, self.tf_dest) + + self.rate.sleep() + + def update_world(self, msg): + + self.world_pose = np.array([msg.pose.pose.position.x, + msg.pose.pose.position.y, + msg.pose.pose.position.z]) + + self.world_ori = [msg.pose.pose.orientation.x, + msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z, + msg.pose.pose.orientation.w] + + def update_odom(self, msg): + + self.odom_pose = np.array([msg.pose.pose.position.x, + msg.pose.pose.position.y, + msg.pose.pose.position.z]) + + self.odom_ori = [msg.pose.pose.orientation.x, + msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z, + -msg.pose.pose.orientation.w] + +if __name__ == '__main__': + + l2t = Link2TF() + l2t.run() \ No newline at end of file diff --git a/swerve_controller/test/config/swervebot.rviz b/swerve_controller/test/config/swervebot.rviz new file mode 100644 index 000000000..f24eaf997 --- /dev/null +++ b/swerve_controller/test/config/swervebot.rviz @@ -0,0 +1,262 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /RobotModel1/Links1/body1 + - /RobotModel1/Links1/leg_lf1 + - /RobotModel1/Links1/leg_lh1 + - /RobotModel1/Links1/leg_rf1 + - /RobotModel1/Links1/leg_rh1 + - /RobotModel1/Links1/wheel_lf1 + - /RobotModel1/Links1/wheel_lh1 + - /RobotModel1/Links1/wheel_rf1 + - /RobotModel1/Links1/wheel_rh1 + Splitter Ratio: 0.5 + Tree Height: 728 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + body: + Alpha: 0.699999988079071 + Show Axes: true + Show Trail: false + Value: true + leg_lf: + Alpha: 0.699999988079071 + Show Axes: true + Show Trail: false + Value: true + leg_lh: + Alpha: 0.699999988079071 + Show Axes: true + Show Trail: false + Value: true + leg_rf: + Alpha: 0.699999988079071 + Show Axes: true + Show Trail: false + Value: true + leg_rh: + Alpha: 0.699999988079071 + Show Axes: true + Show Trail: false + Value: true + wheel_lf: + Alpha: 0.699999988079071 + Show Axes: true + Show Trail: false + Value: true + wheel_lh: + Alpha: 0.699999988079071 + Show Axes: true + Show Trail: false + Value: true + wheel_rf: + Alpha: 0.699999988079071 + Show Axes: true + Show Trail: false + Value: true + wheel_rh: + Alpha: 0.699999988079071 + Show Axes: true + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /swervebot/body_path + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 0; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /swervebot/odom_path + Unreliable: false + Value: true + - Class: rviz/Axes + Enabled: true + Length: 0.20000000298023224 + Name: Axes + Radius: 0.019999999552965164 + Reference Frame: world + Value: true + - Class: rviz/Axes + Enabled: true + Length: 0.20000000298023224 + Name: Axes + Radius: 0.019999999552965164 + Reference Frame: odom + Value: true + Enabled: true + Global Options: + Background Color: 97; 120; 134 + Default Light: true + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 5.4927897453308105 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.71539705991745 + Target Frame: + Value: Orbit (rviz) + Yaw: 2.247215747833252 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1019 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000015600000361fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000361000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000361fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000361000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000006240000036100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 1920 + Y: 33 diff --git a/swerve_controller/test/config/swervebot_control.yaml b/swerve_controller/test/config/swervebot_control.yaml new file mode 100644 index 000000000..36bc57c68 --- /dev/null +++ b/swerve_controller/test/config/swervebot_control.yaml @@ -0,0 +1,39 @@ +swervebot: + # Publish all joint states ----------------------------------- + joint_state_controller: + type: "joint_state_controller/JointStateController" + publish_rate: 100 + + # Controller for independent wheels -------------------------- + swerve_controller: + type: "swerve_controller/SwerveController" + + # Joints + lf_wheel: "wheel_lf_joint" + rf_wheel: "wheel_rf_joint" + lh_wheel: "wheel_lh_joint" + rh_wheel: "wheel_rh_joint" + lf_steering: "leg_lf_joint" + rf_steering: "leg_rf_joint" + lh_steering: "leg_lh_joint" + rh_steering: "leg_rh_joint" + + # Other + publish_rate: 50 + enable_odom_tf: true + pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03] + twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03] + cmd_vel_timeout: 5 + base_frame_id: "body" + + # Low-Level Controllers ------------------------------------- + gazebo_ros_control: + pid_gains: + leg_lf_joint: {p: 50.0, i: 0.5, d: 5} + leg_rf_joint: {p: 50.0, i: 0.5, d: 5} + leg_lh_joint: {p: 50.0, i: 0.5, d: 5} + leg_rh_joint: {p: 50.0, i: 0.5, d: 5} + wheel_lf_joint: {p: 50.0, i: 2.0, d: 0.001} + wheel_rf_joint: {p: 50.0, i: 2.0, d: 0.001} + wheel_lh_joint: {p: 50.0, i: 2.0, d: 0.001} + wheel_rh_joint: {p: 50.0, i: 2.0, d: 0.001} \ No newline at end of file diff --git a/swerve_controller/test/config/swervebot_plotjuggler.xml b/swerve_controller/test/config/swervebot_plotjuggler.xml new file mode 100644 index 000000000..e3206f2fc --- /dev/null +++ b/swerve_controller/test/config/swervebot_plotjuggler.xml @@ -0,0 +1,128 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/swerve_controller/test/include/swerve_controller/swervebot.h b/swerve_controller/test/include/swerve_controller/swervebot.h new file mode 100644 index 000000000..c8eb49025 --- /dev/null +++ b/swerve_controller/test/include/swerve_controller/swervebot.h @@ -0,0 +1,210 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Exobotic + * 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 Exobotic 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. + *********************************************************************/ + + +#ifndef SWERVE_CONTROLLER_SWERVEBOT_H +#define SWERVE_CONTROLLER_SWERVEBOT_H + +#include +#include +#include + +#include +#include +#include + +#include + +// Floating-point value comparison threshold +const double EPS = 0.02; +const double POSITION_TOLERANCE = 0.1; // 10cm/5s = 2 cm-s precision +const double ORIENTATION_TOLERANCE = 0.2; // 0.2rad = 11 degrees precision +const double VELOCITY_TOLERANCE = 0.05; // not consistent (linear and angular speed) + + + +/** + * This class allows to test the swerve controller with google test + */ +class SwerveControllerTest : public ::testing::Test +{ + public: + /** + * \brief Constructor. Create services and topic publishers and subscribers + */ + SwerveControllerTest() + : received_first_odom(false) + , cmd_twist_pub(nh.advertise("cmd_vel", 100)) + , odom_sub(nh.subscribe("odom", 100, &SwerveControllerTest::odomCallback, this)) + , start_srv(nh.serviceClient("start")) + , stop_srv(nh.serviceClient("stop")) + , loop_rate(ros::Rate(40)) + { + } + + /** + * \brief Desctructor. Shuts off the subscriber + */ + ~SwerveControllerTest() + { + odom_sub.shutdown(); + } + + /** + * \brief return le last odometry value + */ + nav_msgs::Odometry getLastOdom() + { + return last_odom; + } + + /** + * \brief Publishes a Twist command in a loop for a given period of time + * \param lin_x linear speed along the X-axis + * \param lin_y linear speed along the Y-axis + * \param ang_z angular speed along the Z-axis + * \param timeout how long to publish the command + */ + void publish(double lin_x, double lin_y, double ang_z, double timeout) + { + geometry_msgs::Twist cmd_vel; + ros::Time start_time = ros::Time::now(); + ros::Duration to(timeout); + while (ros::Time::now() - start_time < to) + { + cmd_vel.linear.x = lin_x; + cmd_vel.linear.y = lin_y; + cmd_vel.angular.z = ang_z; + cmd_twist_pub.publish(cmd_vel); + loop_rate.sleep(); + } + } + + /** + * \brief Check if controller is still alive + */ + const bool isControllerAlive() + { + return (odom_sub.getNumPublishers() > 0) && + ((cmd_twist_pub.getNumSubscribers() > 0) || (cmd_4ws_pub.getNumSubscribers() > 0)); + } + + /** + * \brief Check if the controller has published the first odometry message + */ + const bool hasReceivedFirstOdom() + { + return received_first_odom; + } + + /** + * \brief Start the controller + */ + void start() + { + std_srvs::Empty srv; start_srv.call(srv); + } + + /** + * \brief Stop the controller + */ + void stop() + { + std_srvs::Empty srv; stop_srv.call(srv); + } + + /** + * \brief Wait until controller is started + */ + const void waitForController() + { + while (!isControllerAlive() && ros::ok()) + { + ROS_DEBUG_STREAM_THROTTLE(0.5, "Waiting for controller."); + ros::Duration(0.1).sleep(); + } + if (!ros::ok()) + FAIL() << "Something went wrong while executing test."; + } + + /** + * \brief Wait until the controller has sent the furst odometry message + */ + const void waitForOdomMsgs() + { + while (!hasReceivedFirstOdom() && ros::ok()) + { + ROS_DEBUG_STREAM_THROTTLE(0.5, "Waiting for odom messages to be published."); + ros::Duration(0.01).sleep(); + } + if (!ros::ok()) + FAIL() << "Something went wrong while executing test."; + } + + private: + bool received_first_odom; + ros::NodeHandle nh; + ros::Publisher cmd_twist_pub, cmd_4ws_pub; + ros::Subscriber odom_sub; + nav_msgs::Odometry last_odom; + + ros::ServiceClient start_srv; + ros::ServiceClient stop_srv; + ros::Rate loop_rate; + + /** + * \brief Callback function when odometry message is received + */ + 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; + received_first_odom = true; + } +}; + +/** + * \brief Tansforms a geometry quaternion message to a simple quaternion +* \param quat the geometry_msg quaternion to be transformed +*/ +inline tf::Quaternion tfQuatFromGeomQuat(const geometry_msgs::Quaternion& quat) +{ + return tf::Quaternion(quat.x, quat.y, quat.z, quat.w); +} + +#endif // SWERVE_CONTROLLER_SWERVEBOT_H diff --git a/swerve_controller/test/launch/swervebot.launch b/swerve_controller/test/launch/swervebot.launch new file mode 100644 index 000000000..1ea41da3d --- /dev/null +++ b/swerve_controller/test/launch/swervebot.launch @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ros + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/swerve_controller/test/launch/swervebot_control.launch b/swerve_controller/test/launch/swervebot_control.launch new file mode 100644 index 000000000..46f55c50c --- /dev/null +++ b/swerve_controller/test/launch/swervebot_control.launch @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + 100 + + + + + + + + + + + + + + + + + + + + + + diff --git a/swerve_controller/test/launch/swervebot_plot.launch b/swerve_controller/test/launch/swervebot_plot.launch new file mode 100644 index 000000000..73379be68 --- /dev/null +++ b/swerve_controller/test/launch/swervebot_plot.launch @@ -0,0 +1,6 @@ + + + + + diff --git a/swerve_controller/test/launch/swervebot_rviz.launch b/swerve_controller/test/launch/swervebot_rviz.launch new file mode 100644 index 000000000..6c978821b --- /dev/null +++ b/swerve_controller/test/launch/swervebot_rviz.launch @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/swerve_controller/test/src/swervebot.cpp b/swerve_controller/test/src/swervebot.cpp new file mode 100644 index 000000000..3d36e4b4d --- /dev/null +++ b/swerve_controller/test/src/swervebot.cpp @@ -0,0 +1,478 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Exobotic + * 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 Exobotic 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. + *********************************************************************/ + +#include "swerve_controller/swervebot.h" +#include + + +/* + * Test that swerve_controller is publishing the "odom" frame + * and related transforms + */ +TEST_F(SwerveControllerTest, testOdomFrame) +{ + // Notify the user + ROS_INFO("Test if odom frames shows up!"); + + // Wait unitl ROS shows up + waitForController(); + + // Set up a TF listener + tf::TransformListener listener; + ros::Duration(2.0).sleep(); + + // Check that the odom frame exists + EXPECT_TRUE(listener.frameExists("odom")); +} + +/* + * Navigate in a square and check that we returned in the same place + */ +TEST_F(SwerveControllerTest, testSquare) +{ + // Wait for ROS + waitForController(); + + // Publish a Twist message with zero values + this->publish(0, 0, 0, 0.2); + + // Get initial odometry + nav_msgs::Odometry old_odom = getLastOdom(); + + // Make a square + this->publish(0.5, 0, 0, 2); + this->publish(0, 0.5, 0, 2); + this->publish(-0.5, 0, 0, 2); + this->publish(0, -0.5, 0, 2); + + // Get elapsed time and expected distance + nav_msgs::Odometry new_odom = getLastOdom(); + + // Publish a Twist message with zero values + this->publish(0, 0, 0, 0.2); + + // Compute position difference and angle difference + const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x; + const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y; + const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z; + 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); + + // Check position and velocity errors + EXPECT_LT(fabs(dx), POSITION_TOLERANCE); + EXPECT_LT(fabs(dy), POSITION_TOLERANCE); + EXPECT_LT(fabs(dz), EPS); + EXPECT_LT(fabs(roll_new - roll_old), EPS); + EXPECT_LT(fabs(pitch_new - pitch_old), EPS); + EXPECT_LT(fabs(yaw_new - yaw_old), EPS); + + // Reset gazebo for next test + this->publish(0, 0, 0, 0.2); + ros::Duration(2.0).sleep(); + std_srvs::Empty resetWorldSrv; + ros::service::call("/gazebo/reset_world", resetWorldSrv); + ros::Duration(1.0).sleep(); +} + + +/* + * Set the robot X speed for a given time and check that the robot has moved correctly + */ +TEST_F(SwerveControllerTest, testForwardX) +{ + // Wait for ROS + waitForController(); + + // Publish a Twist message with zero values + this->publish(0, 0, 0, 0.2); + + // Get initial odometry + nav_msgs::Odometry old_odom = getLastOdom(); + + // Set Y speed of 0.2 for 5 seconds + double lin_x = 0.2; + this->publish(lin_x, 0, 0, 5); + + // Get elapsed time and expected distance + nav_msgs::Odometry new_odom = getLastOdom(); + const ros::Duration actual_elapsed_time = new_odom.header.stamp - old_odom.header.stamp; + const double expected_distance = lin_x * actual_elapsed_time.toSec(); + std::cerr << actual_elapsed_time.toSec() << std::endl; + + // Compute position difference and angle difference + const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x; + const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y; + const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z; + 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); + // Check position and velocity errors + EXPECT_NEAR(sqrt(dx*dx + dy*dy), expected_distance, POSITION_TOLERANCE); + EXPECT_LT(fabs(dz), EPS); + + 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), lin_x, VELOCITY_TOLERANCE); + EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS); + EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS); + + EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS); + EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS); + EXPECT_LT(fabs(new_odom.twist.twist.angular.z), EPS); + + // Reset gazebo for next test + this->publish(0, 0, 0, 0.2); + ros::Duration(2.0).sleep(); + std_srvs::Empty resetWorldSrv; + ros::service::call("/gazebo/reset_world", resetWorldSrv); + ros::Duration(1.0).sleep(); +} + + +/* + * Set the robot Y speed for a given time and check that the robot has moved correctly + */ +TEST_F(SwerveControllerTest, testForwardY) +{ + // Wait for ROS + waitForController(); + + // Publish a Twist message with zero values + this->publish(0, 0, 0, 0.2); + + // Get initial odometry + nav_msgs::Odometry old_odom = getLastOdom(); + + // Set Y speed of 0.2 for 5 seconds + double lin_y = 0.2; + this->publish(0, lin_y, 0, 5); + + // Get elapsed time and expected distance + nav_msgs::Odometry new_odom = getLastOdom(); + const ros::Duration actual_elapsed_time = new_odom.header.stamp - old_odom.header.stamp; + const double expected_distance = lin_y * actual_elapsed_time.toSec(); + + // Compute position difference and angle difference + const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x; + const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y; + const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z; + 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); + + // Check position and velocity errors + EXPECT_NEAR(sqrt(dx*dx + dy*dy), expected_distance, POSITION_TOLERANCE); + EXPECT_LT(fabs(dz), EPS); + + 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_LT(fabs(new_odom.twist.twist.linear.x), EPS); + EXPECT_NEAR(fabs(new_odom.twist.twist.linear.y), lin_y, VELOCITY_TOLERANCE); + 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); + + // Reset gazebo for next test + this->publish(0, 0, 0, 0.2); + ros::Duration(2.0).sleep(); + std_srvs::Empty resetWorldSrv; + ros::service::call("/gazebo/reset_world", resetWorldSrv); + ros::Duration(1.0).sleep(); +} + +/* + * Check that the robot is making half a circle clockwise correctly + */ +TEST_F(SwerveControllerTest, testCircleCw) +{ + // Wait for ROS + waitForController(); + + // Publish a Twist message with zero values + this->publish(0, 0, 0, 0.2); + + // Get initial odometry + nav_msgs::Odometry old_odom = getLastOdom(); + + // Turning (linx = pi*r/d and angz= pi/d): + double lin_x = M_PI * 2/ 10; + double ang_z = -M_PI / 10; + this->publish(lin_x, 0, ang_z, 10); + + // Get expected distance + nav_msgs::Odometry new_odom = getLastOdom(); + + // Compute position difference and angle difference + const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x; + const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y; + const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z; + 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); + // Check position and velocity errors + EXPECT_NEAR(sqrt(dx*dx + dy*dy), -2*lin_x/ang_z, 2*POSITION_TOLERANCE); + EXPECT_LT(fabs(dz), EPS); + + EXPECT_LT(fabs(roll_new - roll_old), EPS); + EXPECT_LT(fabs(pitch_new - pitch_old), EPS); + EXPECT_NEAR(fabs(yaw_new - yaw_old), M_PI, ORIENTATION_TOLERANCE); + + EXPECT_NEAR(fabs(new_odom.twist.twist.linear.x), lin_x, VELOCITY_TOLERANCE); + EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS); + EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS); + + EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS); + EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS); + EXPECT_NEAR(new_odom.twist.twist.angular.z, ang_z, VELOCITY_TOLERANCE); + + // Reset gazebo for next test + this->publish(0, 0, 0, 0.2); + ros::Duration(2.0).sleep(); + std_srvs::Empty resetWorldSrv; + ros::service::call("/gazebo/reset_world", resetWorldSrv); + ros::Duration(1.0).sleep(); +} + + +/* + * Check that the robot is making half a circle counter-clockwise correctly + */ +TEST_F(SwerveControllerTest, testCircleCcw) +{ + // Wait for ROS + waitForController(); + + // Publish a Twist message with zero values + this->publish(0, 0, 0, 0.2); + + // Get initial odometry + nav_msgs::Odometry old_odom = getLastOdom(); + + // Turning (linx = pi*r/d and angz= pi/d): + double lin_x = M_PI * 2 / 10; + double ang_z = M_PI / 10; + this->publish(lin_x, 0, ang_z, 10); + + // Get expected distance + nav_msgs::Odometry new_odom = getLastOdom(); + + // Compute position difference and angle difference + const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x; + const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y; + const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z; + 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); + + // Check position and velocity errors + EXPECT_NEAR(sqrt(dx*dx + dy*dy), 2*lin_x/ang_z, 2*POSITION_TOLERANCE); + EXPECT_LT(fabs(dz), EPS); + + EXPECT_LT(fabs(roll_new - roll_old), EPS); + EXPECT_LT(fabs(pitch_new - pitch_old), EPS); + EXPECT_NEAR(fabs(yaw_new - yaw_old), M_PI, ORIENTATION_TOLERANCE); + + EXPECT_NEAR(fabs(new_odom.twist.twist.linear.x), lin_x, VELOCITY_TOLERANCE); + EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS); + EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS); + + EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS); + EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS); + EXPECT_NEAR(new_odom.twist.twist.angular.z, ang_z, VELOCITY_TOLERANCE); + + // Reset gazebo for next test + this->publish(0, 0, 0, 0.2); + ros::Duration(2.0).sleep(); + std_srvs::Empty resetWorldSrv; + ros::service::call("/gazebo/reset_world", resetWorldSrv); + ros::Duration(1.0).sleep(); +} + +/* + * Check that the robot is turning correctly clockwise around its center + */ +TEST_F(SwerveControllerTest, testTurnCw) +{ + // Wait for ROS + waitForController(); + + // Publish a Twist message with zero values + this->publish(0, 0, 0, 0.2); + + // Get initial odometry + nav_msgs::Odometry old_odom = getLastOdom(); + + // Turning a half circle + double ang_z = M_PI / 10; + this->publish(0, 0, ang_z, 10); + + // Get expected distance + nav_msgs::Odometry new_odom = getLastOdom(); + + // Compute position difference and angle difference + const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x; + const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y; + const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z; + 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); + + // Check if position error does not exceed EPS + EXPECT_LT(fabs(dx), EPS); + EXPECT_LT(fabs(dy), EPS); + EXPECT_LT(fabs(dz), EPS); + + 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(new_odom.twist.twist.angular.z, ang_z, VELOCITY_TOLERANCE); + + // Reset gazebo for next test + this->publish(0, 0, 0, 0.2); + ros::Duration(2.0).sleep(); + std_srvs::Empty resetWorldSrv; + ros::service::call("/gazebo/reset_world", resetWorldSrv); + ros::Duration(1.0).sleep(); +} + + +/* + * Check that the robot is turning correctly counter-clockwise around its center + */ +TEST_F(SwerveControllerTest, testTurnCcw) +{ + // Wait for ROS + waitForController(); + + // Publish a Twist message with zero values + this->publish(0, 0, 0, 0.2); + + // Get initial odometry + nav_msgs::Odometry old_odom = getLastOdom(); + + // Turning a full and a half circle + double ang_z = -M_PI / 10; + this->publish(0, 0, ang_z, 10); + + // Get expected distance + nav_msgs::Odometry new_odom = getLastOdom(); + + // Compute position difference and angle difference + const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x; + const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y; + const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z; + 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); + + // Check if position error does not exceed EPS + EXPECT_LT(fabs(dx), EPS); + EXPECT_LT(fabs(dy), EPS); + EXPECT_LT(fabs(dz), EPS); + + 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(new_odom.twist.twist.angular.z, ang_z, VELOCITY_TOLERANCE); + + // Reset gazebo for next test + this->publish(0, 0, 0, 0.2); + ros::Duration(2.0).sleep(); + std_srvs::Empty resetWorldSrv; + ros::service::call("/gazebo/reset_world", resetWorldSrv); + ros::Duration(1.0).sleep(); +} + +int main(int argc, char** argv) +{ + // Initialize the test + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "swervebot_test"); + + // Start spinner + ros::AsyncSpinner spinner(1); + spinner.start(); + + // Run the tests + int ret = RUN_ALL_TESTS(); + + // Stop all + spinner.stop(); + ros::shutdown(); + + return ret; +} diff --git a/swerve_controller/test/swervebot.test b/swerve_controller/test/swervebot.test new file mode 100644 index 000000000..315e9b42c --- /dev/null +++ b/swerve_controller/test/swervebot.test @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/swerve_controller/test/urdf/materials.xacro b/swerve_controller/test/urdf/materials.xacro new file mode 100644 index 000000000..311c3cdd8 --- /dev/null +++ b/swerve_controller/test/urdf/materials.xacro @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/swerve_controller/test/urdf/swervebot.gazebo b/swerve_controller/test/urdf/swervebot.gazebo new file mode 100644 index 000000000..9103c5b0d --- /dev/null +++ b/swerve_controller/test/urdf/swervebot.gazebo @@ -0,0 +1,53 @@ + + + + + + Gazebo/Orange + + + Gazebo/Green + + + Gazebo/Green + + + Gazebo/Green + + + Gazebo/Green + + + Gazebo/Black + + + Gazebo/Black + + + Gazebo/Black + + + Gazebo/Black + + + + + /swervebot + gazebo_ros_control/DefaultRobotHWSim + + + + + + /swervebot + true + 100.0 + body + ground_truth_pose + 0.001 + world + 0 0 0 + 0 0 0 + + + \ No newline at end of file diff --git a/swerve_controller/test/urdf/swervebot.xacro b/swerve_controller/test/urdf/swervebot.xacro new file mode 100644 index 000000000..f0f1adaa7 --- /dev/null +++ b/swerve_controller/test/urdf/swervebot.xacro @@ -0,0 +1,404 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + 1 + + + + diff --git a/swerve_controller/test/worlds/swervebot_normal.world b/swerve_controller/test/worlds/swervebot_normal.world new file mode 100644 index 000000000..1ea853560 --- /dev/null +++ b/swerve_controller/test/worlds/swervebot_normal.world @@ -0,0 +1,24 @@ + + + + + + + model://ground_plane + + + + + model://sun + + + + + + 4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190 + orbit + + + + + From 3bc85e6bf7e9dce3613ec9df6dbef219983c619f Mon Sep 17 00:00:00 2001 From: Gabriel Urbain Date: Mon, 13 Jan 2020 15:16:17 +0100 Subject: [PATCH 2/6] Apply revision comments for Pull Request #441 --- swerve_controller/CMakeLists.txt | 14 +- .../swerve_controller/swerve_controller.h | 49 +------ swerve_controller/test/bin/pub_cmd | 84 ------------ swerve_controller/test/bin/tf_ground_truth | 72 +++++----- .../test/config/swervebot_plotjuggler.xml | 128 ------------------ .../test/launch/swervebot.launch | 2 - .../test/launch/swervebot_rviz.launch | 4 +- swerve_controller/test/urdf/swervebot.xacro | 2 - .../test/worlds/swervebot_normal.world | 24 ---- 9 files changed, 45 insertions(+), 334 deletions(-) delete mode 100755 swerve_controller/test/bin/pub_cmd delete mode 100644 swerve_controller/test/config/swervebot_plotjuggler.xml delete mode 100644 swerve_controller/test/worlds/swervebot_normal.world diff --git a/swerve_controller/CMakeLists.txt b/swerve_controller/CMakeLists.txt index 5d4241648..207248c9b 100644 --- a/swerve_controller/CMakeLists.txt +++ b/swerve_controller/CMakeLists.txt @@ -1,13 +1,8 @@ cmake_minimum_required(VERSION 2.8.3) project(swerve_controller) -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - # Package dependencies -set(${PROJECT_NAME}_CATKIN_DEPS +find_package(catkin REQUIRED COMPONENTS controller_interface joint_state_controller nav_msgs @@ -17,8 +12,6 @@ set(${PROJECT_NAME}_CATKIN_DEPS realtime_tools tf urdf_geometry_parser) -list(SORT ${PROJECT_NAME}_CATKIN_DEPS) -find_package(catkin REQUIRED COMPONENTS ${${PROJECT_NAME}_CATKIN_DEPS}) # Create the package catkin_package( @@ -27,9 +20,8 @@ catkin_package( CATKIN_DEPENDS ${${PROJECT_NAME}_CATKIN_DEPS} ) -# Source and headers for build -include_directories(include) -include_directories(${catkin_INCLUDE_DIRS}) +# Source and headers +include_directories(include ${catkin_INCLUDE_DIRS}) add_library(${PROJECT_NAME} src/odometry.cpp src/speed_limiter.cpp src/swerve_controller.cpp) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/swerve_controller/include/swerve_controller/swerve_controller.h b/swerve_controller/include/swerve_controller/swerve_controller.h index caebc0312..16b27b526 100644 --- a/swerve_controller/include/swerve_controller/swerve_controller.h +++ b/swerve_controller/include/swerve_controller/swerve_controller.h @@ -75,34 +75,15 @@ class SwerveController public: SwerveController(); - /** - * \brief Initialize controller - * \param robot_hw Velocity and position 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::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle &controller_nh); - /** - * \brief Updates controller, i.e. computes the odometry and sets the new velocity commands - * \param time Current time - * \param period Time since the last called to update - */ void update(const ros::Time& time, const ros::Duration& period); - /** - * \brief Starts controller - * \param time Current time - */ void starting(const ros::Time& time); - /** - * \brief Stops controller - * \param time Current time - */ - void stopping(const ros::Time& /*time*/); + void stopping(const ros::Time& time); private: std::string name_; @@ -171,41 +152,17 @@ class SwerveController SpeedLimiter limiter_ang_; private: - /** - * \brief Update and publish odometry - * \param time Current time - */ + void updateOdometry(const ros::Time &time); - /** - * \brief Compute and publish command - * \param time Current time - * \param period Time since the last called to update - */ + void updateCommand(const ros::Time& time, const ros::Duration& period); - /** - * \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 Populates this class with the robot physical parameters at init - * \param controller_nh Node handle inside the controller namespace - */ bool getPhysicalParams(ros::NodeHandle& controller_nh); - /** - * \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); }; diff --git a/swerve_controller/test/bin/pub_cmd b/swerve_controller/test/bin/pub_cmd deleted file mode 100755 index ccfc3d7ea..000000000 --- a/swerve_controller/test/bin/pub_cmd +++ /dev/null @@ -1,84 +0,0 @@ -#! /usr/bin/env python - -########################################################################## -# Software License Agreement (BSD License) -# -# Copyright (c) 2020, Exobotic -# 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 Exobotic 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. -########################################################################## - -import copy -from geometry_msgs.msg import Twist -from nav_msgs.msg import Odometry -import rospy as ros -import time - -def odomCallback(odom): - global last_x - last_x = odom.pose.pose.position.x - -if __name__ == "__main__": - - global last_x - last_x = 0 - ros.init_node("test_pub") - pub = ros.Publisher("/swervebot/swerve_controller/cmd_vel", Twist, queue_size=1) - sub = ros.Subscriber("/swervebot/swerve_controller/odom", Odometry, odomCallback) - - - cmd_vel = Twist() - cmd_vel.linear.x = 0.0 - cmd_vel.linear.y = 0.0 - cmd_vel.angular.z = 0.0 - - prev_time = time.time() - while (time.time() - prev_time < 0.1) and not ros.is_shutdown(): - cmd_vel.linear.x = 0.5 - pub.publish(cmd_vel) - - - init_x = copy.copy(last_x) - cmd_vel.linear.x = 0.5 - prev_time = time.time() - while (time.time() - prev_time < 10) and not ros.is_shutdown(): - pub.publish(cmd_vel) - - cmd_vel.linear.x = 0.0 - prev_time = time.time() - while (time.time() - prev_time < 0.1) and not ros.is_shutdown(): - cmd_vel.linear.x = 0.5 - pub.publish(cmd_vel) - - final_x = copy.copy(last_x) - - print "DIFF: " + str(final_x - init_x) - print final_x, init_x - - diff --git a/swerve_controller/test/bin/tf_ground_truth b/swerve_controller/test/bin/tf_ground_truth index a1d352be9..f8e995537 100755 --- a/swerve_controller/test/bin/tf_ground_truth +++ b/swerve_controller/test/bin/tf_ground_truth @@ -42,55 +42,55 @@ from nav_msgs.msg import Odometry class Link2TF(): - def __init__(self): + def __init__(self): - ros.init_node("tf_world_publisher") + ros.init_node("tf_world_publisher") - self.rate = ros.Rate(100) - self.tf_pub = tf.TransformBroadcaster() - self.tf_orig = "odom" - self.tf_dest = "world" - self.world_sub = ros.Subscriber("/swervebot/ground_truth_pose", Odometry, self.update_world) - self.odom_sub = ros.Subscriber("/swervebot/swerve_controller/odom", Odometry, self.update_odom) + self.rate = ros.Rate(100) + self.tf_pub = tf.TransformBroadcaster() + self.tf_orig = "odom" + self.tf_dest = "world" + self.world_sub = ros.Subscriber("/swervebot/ground_truth_pose", Odometry, self.update_world) + self.odom_sub = ros.Subscriber("/swervebot/swerve_controller/odom", Odometry, self.update_odom) - self.world_pose = np.array([0, 0, 0]) - self.odom_pose = np.array([0, 0, 0]) - self.world_ori = [0, 0, 0, 0] - self.odom_ori = [0, 0, 0, 0] + self.world_pose = np.array([0, 0, 0]) + self.odom_pose = np.array([0, 0, 0]) + self.world_ori = [0, 0, 0, 0] + self.odom_ori = [0, 0, 0, 0] - def run(self): + def run(self): - while not ros.is_shutdown(): + while not ros.is_shutdown(): - self.tf_pub.sendTransform(self.world_pose - self.odom_pose, - tf.transformations.quaternion_multiply(self.world_ori, self.odom_ori), - ros.Time.now(), self.tf_orig, self.tf_dest) + self.tf_pub.sendTransform(self.world_pose - self.odom_pose, + tf.transformations.quaternion_multiply(self.world_ori, self.odom_ori), + ros.Time.now(), self.tf_orig, self.tf_dest) - self.rate.sleep() + self.rate.sleep() - def update_world(self, msg): + def update_world(self, msg): - self.world_pose = np.array([msg.pose.pose.position.x, - msg.pose.pose.position.y, - msg.pose.pose.position.z]) + self.world_pose = np.array([msg.pose.pose.position.x, + msg.pose.pose.position.y, + msg.pose.pose.position.z]) - self.world_ori = [msg.pose.pose.orientation.x, - msg.pose.pose.orientation.y, - msg.pose.pose.orientation.z, - msg.pose.pose.orientation.w] + self.world_ori = [msg.pose.pose.orientation.x, + msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z, + msg.pose.pose.orientation.w] - def update_odom(self, msg): + def update_odom(self, msg): - self.odom_pose = np.array([msg.pose.pose.position.x, - msg.pose.pose.position.y, - msg.pose.pose.position.z]) + self.odom_pose = np.array([msg.pose.pose.position.x, + msg.pose.pose.position.y, + msg.pose.pose.position.z]) - self.odom_ori = [msg.pose.pose.orientation.x, - msg.pose.pose.orientation.y, - msg.pose.pose.orientation.z, - -msg.pose.pose.orientation.w] + self.odom_ori = [msg.pose.pose.orientation.x, + msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z, + -msg.pose.pose.orientation.w] if __name__ == '__main__': - l2t = Link2TF() - l2t.run() \ No newline at end of file + l2t = Link2TF() + l2t.run() \ No newline at end of file diff --git a/swerve_controller/test/config/swervebot_plotjuggler.xml b/swerve_controller/test/config/swervebot_plotjuggler.xml deleted file mode 100644 index e3206f2fc..000000000 --- a/swerve_controller/test/config/swervebot_plotjuggler.xml +++ /dev/null @@ -1,128 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/swerve_controller/test/launch/swervebot.launch b/swerve_controller/test/launch/swervebot.launch index 1ea41da3d..854594408 100644 --- a/swerve_controller/test/launch/swervebot.launch +++ b/swerve_controller/test/launch/swervebot.launch @@ -1,6 +1,5 @@ - @@ -29,7 +28,6 @@ - diff --git a/swerve_controller/test/launch/swervebot_rviz.launch b/swerve_controller/test/launch/swervebot_rviz.launch index 6c978821b..58d293afe 100644 --- a/swerve_controller/test/launch/swervebot_rviz.launch +++ b/swerve_controller/test/launch/swervebot_rviz.launch @@ -17,6 +17,7 @@ + @@ -24,6 +25,7 @@ + @@ -32,4 +34,4 @@ - \ No newline at end of file + diff --git a/swerve_controller/test/urdf/swervebot.xacro b/swerve_controller/test/urdf/swervebot.xacro index f0f1adaa7..a93818da1 100644 --- a/swerve_controller/test/urdf/swervebot.xacro +++ b/swerve_controller/test/urdf/swervebot.xacro @@ -1,5 +1,4 @@ - - diff --git a/swerve_controller/test/worlds/swervebot_normal.world b/swerve_controller/test/worlds/swervebot_normal.world deleted file mode 100644 index 1ea853560..000000000 --- a/swerve_controller/test/worlds/swervebot_normal.world +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - model://ground_plane - - - - - model://sun - - - - - - 4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190 - orbit - - - - - From d9b6be371cfd720593124729ace724ab0586bbf6 Mon Sep 17 00:00:00 2001 From: Gabriel Urbain Date: Wed, 8 Jul 2020 18:16:43 +0200 Subject: [PATCH 3/6] Enhancement features suggested by @AravindaDP in ros-controllers PR #441 --- .../swerve_controller/swerve_controller.h | 48 ++-- swerve_controller/src/swerve_controller.cpp | 243 +++++++++++------- .../test/config/swervebot_control.yaml | 4 + .../include/swerve_controller/swervebot.h | 202 +++++++-------- 4 files changed, 279 insertions(+), 218 deletions(-) diff --git a/swerve_controller/include/swerve_controller/swerve_controller.h b/swerve_controller/include/swerve_controller/swerve_controller.h index 16b27b526..15ce0b40a 100644 --- a/swerve_controller/include/swerve_controller/swerve_controller.h +++ b/swerve_controller/include/swerve_controller/swerve_controller.h @@ -59,7 +59,7 @@ namespace swerve_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 @@ -68,22 +68,22 @@ namespace swerve_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 SwerveController + class SwerveController : public controller_interface::MultiInterfaceController -{ + { public: SwerveController(); - bool init(hardware_interface::RobotHW* robot_hw, - ros::NodeHandle& root_nh, - ros::NodeHandle &controller_nh); + bool init(hardware_interface::RobotHW *robot_hw, + ros::NodeHandle &root_nh, + ros::NodeHandle &controller_nh); - void update(const ros::Time& time, const ros::Duration& period); + void update(const ros::Time &time, const ros::Duration &period); - void starting(const ros::Time& time); + void starting(const ros::Time &time); - void stopping(const ros::Time& time); + void stopping(const ros::Time &time); private: std::string name_; @@ -118,8 +118,8 @@ class SwerveController ros::Subscriber sub_command_; /// Odometry related: - std::shared_ptr > odom_pub_; - std::shared_ptr > tf_odom_pub_; + std::shared_ptr> odom_pub_; + std::shared_ptr> tf_odom_pub_; Odometry odometry_; /// Wheel separation (or track), distance between left and right wheels @@ -136,6 +136,9 @@ class SwerveController /// Wheel base (distance between front and rear wheel): double wheel_base_; + /// Range of steering angle: + double min_steering_angle_, max_steering_angle_; + /// Timeout to consider cmd_vel commands old: double cmd_vel_timeout_; @@ -152,23 +155,24 @@ class SwerveController SpeedLimiter limiter_ang_; private: - void updateOdometry(const ros::Time &time); - - void updateCommand(const ros::Time& time, const ros::Duration& period); + + void updateCommand(const ros::Time &time, const ros::Duration &period); void brake(); - void cmdVelCallback(const geometry_msgs::Twist& command); + bool clipSteeringAngle(double &steering, double &speed); + + void cmdVelCallback(const geometry_msgs::Twist &command); - bool getPhysicalParams(ros::NodeHandle& controller_nh); + bool getPhysicalParams(ros::NodeHandle &controller_nh); - void setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh); -}; + void setOdomPubFields(ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh); + }; -PLUGINLIB_EXPORT_CLASS(swerve_controller::SwerveController, - controller_interface::ControllerBase); + PLUGINLIB_EXPORT_CLASS(swerve_controller::SwerveController, + controller_interface::ControllerBase); -} // namespace swerve_controller +} // namespace swerve_controller -#endif // SWERVE_CONTROLLER_SWERVE_CONTROLLER_H +#endif // SWERVE_CONTROLLER_SWERVE_CONTROLLER_H diff --git a/swerve_controller/src/swerve_controller.cpp b/swerve_controller/src/swerve_controller.cpp index 40db3b0a8..f44fd0f71 100644 --- a/swerve_controller/src/swerve_controller.cpp +++ b/swerve_controller/src/swerve_controller.cpp @@ -41,19 +41,21 @@ namespace swerve_controller { SwerveController::SwerveController() - : command_struct_twist_() - , track_(0.0) - , wheel_steering_y_offset_(0.0) - , wheel_radius_(0.0) - , wheel_base_(0.0) - , cmd_vel_timeout_(0.5) - , base_frame_id_("base_link") - , enable_odom_tf_(true) + : command_struct_twist_(), + track_(0.0), + wheel_steering_y_offset_(0.0), + wheel_radius_(0.0), + wheel_base_(0.0), + min_steering_angle_(-M_PI), + max_steering_angle_(M_PI), + cmd_vel_timeout_(0.5), + base_frame_id_("base_link"), + enable_odom_tf_(true) { } bool SwerveController::init(hardware_interface::RobotHW *robot_hw, - ros::NodeHandle& root_nh, + ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh) { // Get namespace @@ -69,7 +71,7 @@ namespace swerve_controller !controller_nh.param("rh_wheel", rh_wheel_name, rh_wheel_name)) { ROS_ERROR_STREAM_NAMED(name_, - "Couldn't retrieve wheel joint param !"); + "Couldn't retrieve wheel joint params !"); return false; } @@ -81,57 +83,71 @@ namespace swerve_controller !controller_nh.param("rh_steering", rh_steering_name, rh_steering_name)) { ROS_ERROR_STREAM_NAMED(name_, - "Couldn't retrieve steering joint param !"); + "Couldn't retrieve steering joint params !"); return false; } + // Get maximal steering angle from the parameter server + controller_nh.param("min_steering_angle", min_steering_angle_, min_steering_angle_); + controller_nh.param("max_steering_angle", max_steering_angle_, max_steering_angle_); + // Get publisher related from the parameter server 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_rate << "Hz."); publish_period_ = ros::Duration(1.0 / publish_rate); - // Get twist related from the parameter server 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 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")); + ROS_INFO_STREAM_NAMED(name_, "Publishing to tf is " + << (enable_odom_tf_ ? "enabled" : "disabled")); // Get velocity and acceleration limits from the parameter server controller_nh.param("linear/x/has_velocity_limits", - limiter_lin_.has_velocity_limits, limiter_lin_.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); + 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); + limiter_lin_.max_velocity, + limiter_lin_.max_velocity); controller_nh.param("linear/x/min_velocity", - limiter_lin_.min_velocity, -limiter_lin_.max_velocity); + limiter_lin_.min_velocity, + -limiter_lin_.max_velocity); controller_nh.param("linear/x/max_acceleration", - limiter_lin_.max_acceleration, limiter_lin_.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); + 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); + 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); + limiter_ang_.max_velocity, + limiter_ang_.max_velocity); controller_nh.param("angular/z/min_velocity", - limiter_ang_.min_velocity, -limiter_ang_.max_velocity); + limiter_ang_.min_velocity, + -limiter_ang_.max_velocity); controller_nh.param("angular/z/max_acceleration", - limiter_ang_.max_acceleration, limiter_ang_.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); + limiter_ang_.min_acceleration, + -limiter_ang_.max_acceleration); // Get robot physical parameters from URDF or parameter server bool lookup_track = !controller_nh.getParam("track", track_); @@ -146,7 +162,7 @@ namespace swerve_controller controller_nh.setParam("track", track_); if (!uvk.getDistanceBetweenJoints(lf_steering_name, lf_wheel_name, - wheel_steering_y_offset_)) + wheel_steering_y_offset_)) return false; else controller_nh.setParam("wheel_steering_y_offset", wheel_steering_y_offset_); @@ -159,44 +175,46 @@ namespace swerve_controller if (lookup_wheel_base) if (!uvk.getDistanceBetweenJoints(lf_wheel_name, lh_wheel_name, - wheel_base_)) + wheel_base_)) return false; else controller_nh.setParam("wheel_base", wheel_base_); // Set physical parameters in odometry - odometry_.setWheelParams(track_-2*wheel_steering_y_offset_, wheel_radius_, wheel_base_); - ROS_INFO_STREAM_NAMED(name_, "Odometry params : track " << track_ << ", wheel radius " - << wheel_radius_ << ", wheel base " << wheel_base_ - << ", wheel steering offset " << wheel_steering_y_offset_); + odometry_.setWheelParams(track_ - 2 * wheel_steering_y_offset_, wheel_radius_, wheel_base_); + ROS_INFO_STREAM_NAMED(name_, "Odometry params : track " + << track_ << ", wheel radius " + << wheel_radius_ << ", wheel base " << wheel_base_ + << ", wheel steering offset " << wheel_steering_y_offset_); setOdomPubFields(root_nh, controller_nh); - // Get hardware interface hardware_interface::VelocityJointInterface *const vel_joint_hw = - robot_hw->get(); + robot_hw->get(); hardware_interface::PositionJointInterface *const pos_joint_hw = - robot_hw->get(); + robot_hw->get(); // Get the wheel joint object to use in the realtime loop - ROS_INFO_STREAM_NAMED(name_, "Adding LF wheel with joint name: " << lf_wheel_name - << ", RF wheel with joint name: " << rf_wheel_name - << ", LH wheel with joint name: " << lh_wheel_name - << ", RH wheel with joint name: " << rh_wheel_name); - lf_wheel_joint_ = vel_joint_hw->getHandle(lf_wheel_name); // throws on failure - rf_wheel_joint_ = vel_joint_hw->getHandle(rf_wheel_name); // throws on failure - lh_wheel_joint_ = vel_joint_hw->getHandle(lh_wheel_name); // throws on failure - rh_wheel_joint_ = vel_joint_hw->getHandle(rh_wheel_name); // throws on failure + ROS_INFO_STREAM_NAMED(name_, "Adding LF wheel with joint name: " + << lf_wheel_name + << ", RF wheel with joint name: " << rf_wheel_name + << ", LH wheel with joint name: " << lh_wheel_name + << ", RH wheel with joint name: " << rh_wheel_name); + lf_wheel_joint_ = vel_joint_hw->getHandle(lf_wheel_name); // throws on failure + rf_wheel_joint_ = vel_joint_hw->getHandle(rf_wheel_name); // throws on failure + lh_wheel_joint_ = vel_joint_hw->getHandle(lh_wheel_name); // throws on failure + rh_wheel_joint_ = vel_joint_hw->getHandle(rh_wheel_name); // throws on failure // Get the steering joint object to use in the realtime loop - ROS_INFO_STREAM_NAMED(name_, "Adding LF steering with joint name: " << lf_steering_name - << ", RF steering with joint name: " << rf_steering_name - << ", LH steering with joint name: " << lh_steering_name - << ", RH steering with joint name: " << rh_steering_name); - lf_steering_joint_ = pos_joint_hw->getHandle(lf_steering_name); // throws on failure - rf_steering_joint_ = pos_joint_hw->getHandle(rf_steering_name); // throws on failure - lh_steering_joint_ = pos_joint_hw->getHandle(lh_steering_name); // throws on failure - rh_steering_joint_ = pos_joint_hw->getHandle(rh_steering_name); // throws on failure + ROS_INFO_STREAM_NAMED(name_, "Adding LF steering with joint name: " + << lf_steering_name + << ", RF steering with joint name: " << rf_steering_name + << ", LH steering with joint name: " << lh_steering_name + << ", RH steering with joint name: " << rh_steering_name); + lf_steering_joint_ = pos_joint_hw->getHandle(lf_steering_name); // throws on failure + rf_steering_joint_ = pos_joint_hw->getHandle(rf_steering_name); // throws on failure + lh_steering_joint_ = pos_joint_hw->getHandle(lh_steering_name); // throws on failure + rh_steering_joint_ = pos_joint_hw->getHandle(rh_steering_name); // throws on failure // Subscribe to Twist messages sub_command_ = controller_nh.subscribe("cmd_vel", 1, @@ -204,13 +222,13 @@ namespace swerve_controller return true; } - void SwerveController::update(const ros::Time& time, const ros::Duration& period) + void SwerveController::update(const ros::Time &time, const ros::Duration &period) { updateOdometry(time); updateCommand(time, period); } - void SwerveController::starting(const ros::Time& time) + void SwerveController::starting(const ros::Time &time) { brake(); @@ -220,28 +238,26 @@ namespace swerve_controller odometry_.init(time); } - void SwerveController::stopping(const ros::Time& time) + void SwerveController::stopping(const ros::Time &time) { brake(); } - void SwerveController::updateOdometry(const ros::Time& time) + void SwerveController::updateOdometry(const ros::Time &time) { // Get current position and velocities const double lf_speed = lf_wheel_joint_->getVelocity(); const double rf_speed = rf_wheel_joint_->getVelocity(); const double lh_speed = lh_wheel_joint_->getVelocity(); const double rh_speed = rh_wheel_joint_->getVelocity(); - if (std::isnan(lf_speed) || std::isnan(rf_speed) - || std::isnan(lh_speed) || std::isnan(rh_speed)) + if (std::isnan(lf_speed) || std::isnan(rf_speed) || std::isnan(lh_speed) || std::isnan(rh_speed)) return; const double lf_steering = lf_steering_joint_->getPosition(); const double rf_steering = rf_steering_joint_->getPosition(); const double lh_steering = lh_steering_joint_->getPosition(); const double rh_steering = rh_steering_joint_->getPosition(); - if (std::isnan(lf_steering) || std::isnan(rf_steering) - || std::isnan(lh_steering) || std::isnan(rh_steering)) + if (std::isnan(lf_steering) || std::isnan(rf_steering) || std::isnan(lh_steering) || std::isnan(rh_steering)) return; // Estimate linear and angular velocity using joint information @@ -255,7 +271,7 @@ namespace swerve_controller // Compute and store orientation info const geometry_msgs::Quaternion orientation( - tf::createQuaternionMsgFromYaw(odometry_.getHeading())); + tf::createQuaternionMsgFromYaw(odometry_.getHeading())); // Populate odom message and publish if (odom_pub_->trylock()) @@ -264,8 +280,8 @@ namespace swerve_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_.getLinearX(); - odom_pub_->msg_.twist.twist.linear.y = odometry_.getLinearY(); + 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(); } @@ -273,7 +289,7 @@ namespace swerve_controller // Publish tf /odom frame if (enable_odom_tf_ && tf_odom_pub_->trylock()) { - geometry_msgs::TransformStamped& odom_frame = tf_odom_pub_->msg_.transforms[0]; + 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(); @@ -283,7 +299,7 @@ namespace swerve_controller } } - void SwerveController::updateCommand(const ros::Time& time, const ros::Duration& period) + void SwerveController::updateCommand(const ros::Time &time, const ros::Duration &period) { // Retreive current velocity command and time step CommandTwist curr_cmd = *(command_twist_.readFromRT()); @@ -298,7 +314,7 @@ namespace swerve_controller // Create velocities and position variables const double cmd_dt(period.toSec()); - const double steering_track = track_- 2 * wheel_steering_y_offset_; + const double steering_track = track_ - 2 * wheel_steering_y_offset_; double lf_speed = 0, rf_speed = 0, lh_speed = 0, rh_speed = 0; double lf_steering = 0, rf_steering = 0, lh_steering = 0, rh_steering = 0; @@ -311,7 +327,7 @@ namespace swerve_controller // Compute wheels velocities and steering angles if ((fabs(curr_cmd.lin_x) > 0.001) || (fabs(curr_cmd.lin_y) > 0.001) || - (fabs(curr_cmd.ang) > 0.001)) + (fabs(curr_cmd.ang) > 0.001)) { double a = curr_cmd.lin_y - curr_cmd.ang * wheel_base_ / 2; double b = curr_cmd.lin_y + curr_cmd.ang * wheel_base_ / 2; @@ -329,6 +345,16 @@ namespace swerve_controller rh_steering = atan2(a, d); } + // Invert wheel speed or brake if steering angle exceeds desired limits + if (!clipSteeringAngle(lf_steering, lf_speed) || + !clipSteeringAngle(rf_steering, rf_speed) || + !clipSteeringAngle(lh_steering, lh_speed) || + !clipSteeringAngle(rh_steering, rh_speed)) + { + brake(); + return; + } + // Set wheels velocities if (lf_wheel_joint_ && rf_wheel_joint_ && lh_wheel_joint_ && rh_wheel_joint_) { @@ -339,7 +365,7 @@ namespace swerve_controller } // Set wheels steering angles - if (lf_wheel_joint_ && rf_wheel_joint_ && lh_wheel_joint_ && rh_wheel_joint_) + if (lf_steering_joint_ && rf_steering_joint_ && lh_steering_joint_ && rh_steering_joint_) { lf_steering_joint_->setCommand(lf_steering); rf_steering_joint_->setCommand(rf_steering); @@ -369,7 +395,40 @@ namespace swerve_controller } } - void SwerveController::cmdVelCallback(const geometry_msgs::Twist& command) + bool SwerveController::clipSteeringAngle(double &steering, double &speed) + { + if (steering > max_steering_angle_) + { + if (steering - M_PI > min_steering_angle_) + { + steering -= M_PI; + speed = -speed; + return true; + } + else + { + return false; + } + } + + if (steering < min_steering_angle_) + { + if (steering + M_PI < max_steering_angle_) + { + steering += M_PI; + speed = -speed; + return true; + } + else + { + return false; + } + } + + return true; + } + + void SwerveController::cmdVelCallback(const geometry_msgs::Twist &command) { if (isRunning()) { @@ -383,11 +442,11 @@ namespace swerve_controller command_struct_twist_.lin_y = command.linear.y; command_struct_twist_.stamp = ros::Time::now(); command_twist_.writeFromNonRT(command_struct_twist_); - ROS_DEBUG_STREAM_NAMED(name_, "Added values to command. " << "Ang: " << - command_struct_twist_.ang << ", Lin x: " << - command_struct_twist_.lin_x << ", Lin y: " << - command_struct_twist_.lin_y << ", Stamp: " << - command_struct_twist_.stamp); + ROS_DEBUG_STREAM_NAMED(name_, "Added values to command. " + << "Ang: " << command_struct_twist_.ang + << ", Lin x: " << command_struct_twist_.lin_x + << ", Lin y: " << command_struct_twist_.lin_y + << ", Stamp: " << command_struct_twist_.stamp); } else { @@ -395,8 +454,8 @@ namespace swerve_controller } } - void SwerveController::setOdomPubFields(ros::NodeHandle& root_nh, - ros::NodeHandle& controller_nh) + void SwerveController::setOdomPubFields(ros::NodeHandle &root_nh, + ros::NodeHandle &controller_nh) { // Get and check params for covariances XmlRpc::XmlRpcValue pose_cov_list; @@ -420,23 +479,23 @@ namespace swerve_controller odom_pub_->msg_.child_frame_id = base_frame_id_; odom_pub_->msg_.pose.pose.position.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; + {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 = - { 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]) }; + {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)); @@ -446,4 +505,4 @@ namespace swerve_controller tf_odom_pub_->msg_.transforms[0].header.frame_id = "odom"; } -} // namespace swerve_controller +} // namespace swerve_controller diff --git a/swerve_controller/test/config/swervebot_control.yaml b/swerve_controller/test/config/swervebot_control.yaml index 36bc57c68..157b362ea 100644 --- a/swerve_controller/test/config/swervebot_control.yaml +++ b/swerve_controller/test/config/swervebot_control.yaml @@ -18,6 +18,10 @@ swervebot: lh_steering: "leg_lh_joint" rh_steering: "leg_rh_joint" + # Range of motion of steering motors + min_steering_angle: -1.58 + max_steering_angle: 1.58 + # Other publish_rate: 50 enable_odom_tf: true diff --git a/swerve_controller/test/include/swerve_controller/swervebot.h b/swerve_controller/test/include/swerve_controller/swervebot.h index c8eb49025..364e85ea6 100644 --- a/swerve_controller/test/include/swerve_controller/swervebot.h +++ b/swerve_controller/test/include/swerve_controller/swervebot.h @@ -33,7 +33,6 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ - #ifndef SWERVE_CONTROLLER_SWERVEBOT_H #define SWERVE_CONTROLLER_SWERVEBOT_H @@ -48,163 +47,158 @@ #include // Floating-point value comparison threshold -const double EPS = 0.02; -const double POSITION_TOLERANCE = 0.1; // 10cm/5s = 2 cm-s precision -const double ORIENTATION_TOLERANCE = 0.2; // 0.2rad = 11 degrees precision -const double VELOCITY_TOLERANCE = 0.05; // not consistent (linear and angular speed) - - +const double EPS = 0.025; +const double POSITION_TOLERANCE = 0.1; // 10cm/5s = 2 cm-s precision +const double ORIENTATION_TOLERANCE = 0.2; // 0.2rad = 11 degrees precision +const double VELOCITY_TOLERANCE = 0.05; // not consistent (linear and angular speed) /** * This class allows to test the swerve controller with google test */ class SwerveControllerTest : public ::testing::Test { - public: - /** +public: + /** * \brief Constructor. Create services and topic publishers and subscribers */ - SwerveControllerTest() - : received_first_odom(false) - , cmd_twist_pub(nh.advertise("cmd_vel", 100)) - , odom_sub(nh.subscribe("odom", 100, &SwerveControllerTest::odomCallback, this)) - , start_srv(nh.serviceClient("start")) - , stop_srv(nh.serviceClient("stop")) - , loop_rate(ros::Rate(40)) - { - } + SwerveControllerTest() + : received_first_odom(false), cmd_twist_pub(nh.advertise("cmd_vel", 100)), odom_sub(nh.subscribe("odom", 100, &SwerveControllerTest::odomCallback, this)), start_srv(nh.serviceClient("start")), stop_srv(nh.serviceClient("stop")), loop_rate(ros::Rate(40)) + { + } - /** + /** * \brief Desctructor. Shuts off the subscriber */ - ~SwerveControllerTest() - { - odom_sub.shutdown(); - } + ~SwerveControllerTest() + { + odom_sub.shutdown(); + } - /** + /** * \brief return le last odometry value */ - nav_msgs::Odometry getLastOdom() - { - return last_odom; - } + nav_msgs::Odometry getLastOdom() + { + return last_odom; + } - /** + /** * \brief Publishes a Twist command in a loop for a given period of time * \param lin_x linear speed along the X-axis * \param lin_y linear speed along the Y-axis * \param ang_z angular speed along the Z-axis * \param timeout how long to publish the command */ - void publish(double lin_x, double lin_y, double ang_z, double timeout) + void publish(double lin_x, double lin_y, double ang_z, double timeout) + { + geometry_msgs::Twist cmd_vel; + ros::Time start_time = ros::Time::now(); + ros::Duration to(timeout); + while (ros::Time::now() - start_time < to) { - geometry_msgs::Twist cmd_vel; - ros::Time start_time = ros::Time::now(); - ros::Duration to(timeout); - while (ros::Time::now() - start_time < to) - { - cmd_vel.linear.x = lin_x; - cmd_vel.linear.y = lin_y; - cmd_vel.angular.z = ang_z; - cmd_twist_pub.publish(cmd_vel); - loop_rate.sleep(); - } + cmd_vel.linear.x = lin_x; + cmd_vel.linear.y = lin_y; + cmd_vel.angular.z = ang_z; + cmd_twist_pub.publish(cmd_vel); + loop_rate.sleep(); } + } - /** + /** * \brief Check if controller is still alive */ - const bool isControllerAlive() - { - return (odom_sub.getNumPublishers() > 0) && - ((cmd_twist_pub.getNumSubscribers() > 0) || (cmd_4ws_pub.getNumSubscribers() > 0)); - } + const bool isControllerAlive() + { + return (odom_sub.getNumPublishers() > 0) && + ((cmd_twist_pub.getNumSubscribers() > 0) || (cmd_4ws_pub.getNumSubscribers() > 0)); + } - /** + /** * \brief Check if the controller has published the first odometry message */ - const bool hasReceivedFirstOdom() - { - return received_first_odom; - } + const bool hasReceivedFirstOdom() + { + return received_first_odom; + } - /** + /** * \brief Start the controller */ - void start() - { - std_srvs::Empty srv; start_srv.call(srv); - } + void start() + { + std_srvs::Empty srv; + start_srv.call(srv); + } - /** + /** * \brief Stop the controller */ - void stop() - { - std_srvs::Empty srv; stop_srv.call(srv); - } + void stop() + { + std_srvs::Empty srv; + stop_srv.call(srv); + } - /** + /** * \brief Wait until controller is started */ - const void waitForController() + const void waitForController() + { + while (!isControllerAlive() && ros::ok()) { - while (!isControllerAlive() && ros::ok()) - { - ROS_DEBUG_STREAM_THROTTLE(0.5, "Waiting for controller."); - ros::Duration(0.1).sleep(); - } - if (!ros::ok()) - FAIL() << "Something went wrong while executing test."; + ROS_DEBUG_STREAM_THROTTLE(0.5, "Waiting for controller."); + ros::Duration(0.1).sleep(); } + if (!ros::ok()) + FAIL() << "Something went wrong while executing test."; + } - /** + /** * \brief Wait until the controller has sent the furst odometry message */ - const void waitForOdomMsgs() + const void waitForOdomMsgs() + { + while (!hasReceivedFirstOdom() && ros::ok()) { - while (!hasReceivedFirstOdom() && ros::ok()) - { - ROS_DEBUG_STREAM_THROTTLE(0.5, "Waiting for odom messages to be published."); - ros::Duration(0.01).sleep(); - } - if (!ros::ok()) - FAIL() << "Something went wrong while executing test."; + ROS_DEBUG_STREAM_THROTTLE(0.5, "Waiting for odom messages to be published."); + ros::Duration(0.01).sleep(); } - - private: - bool received_first_odom; - ros::NodeHandle nh; - ros::Publisher cmd_twist_pub, cmd_4ws_pub; - ros::Subscriber odom_sub; - nav_msgs::Odometry last_odom; - - ros::ServiceClient start_srv; - ros::ServiceClient stop_srv; - ros::Rate loop_rate; - - /** + if (!ros::ok()) + FAIL() << "Something went wrong while executing test."; + } + +private: + bool received_first_odom; + ros::NodeHandle nh; + ros::Publisher cmd_twist_pub, cmd_4ws_pub; + ros::Subscriber odom_sub; + nav_msgs::Odometry last_odom; + + ros::ServiceClient start_srv; + ros::ServiceClient stop_srv; + ros::Rate loop_rate; + + /** * \brief Callback function when odometry message is received */ - 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; - received_first_odom = true; - } + 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; + received_first_odom = true; + } }; /** * \brief Tansforms a geometry quaternion message to a simple quaternion * \param quat the geometry_msg quaternion to be transformed */ -inline tf::Quaternion tfQuatFromGeomQuat(const geometry_msgs::Quaternion& quat) +inline tf::Quaternion tfQuatFromGeomQuat(const geometry_msgs::Quaternion &quat) { return tf::Quaternion(quat.x, quat.y, quat.z, quat.w); } -#endif // SWERVE_CONTROLLER_SWERVEBOT_H +#endif // SWERVE_CONTROLLER_SWERVEBOT_H From 9c3d2956c1e248711c21375349f4ac0f7b457d73 Mon Sep 17 00:00:00 2001 From: Gabriel Urbain Date: Mon, 10 Aug 2020 12:56:19 +0200 Subject: [PATCH 4/6] remove steering angles reset when braking --- swerve_controller/src/swerve_controller.cpp | 9 --------- 1 file changed, 9 deletions(-) diff --git a/swerve_controller/src/swerve_controller.cpp b/swerve_controller/src/swerve_controller.cpp index f44fd0f71..ebefa4b9a 100644 --- a/swerve_controller/src/swerve_controller.cpp +++ b/swerve_controller/src/swerve_controller.cpp @@ -384,15 +384,6 @@ namespace swerve_controller lh_wheel_joint_->setCommand(0.0); rh_wheel_joint_->setCommand(0.0); } - - // Set wheels steering angles - if (lf_wheel_joint_ && rf_wheel_joint_ && lh_wheel_joint_ && rh_wheel_joint_) - { - lf_steering_joint_->setCommand(0.0); - rf_steering_joint_->setCommand(0.0); - lh_steering_joint_->setCommand(0.0); - rh_steering_joint_->setCommand(0.0); - } } bool SwerveController::clipSteeringAngle(double &steering, double &speed) From 0346bf75d96e38f084d10206e13bad75ca2da0c9 Mon Sep 17 00:00:00 2001 From: Gabriel Urbain Date: Mon, 10 Aug 2020 14:19:37 +0200 Subject: [PATCH 5/6] remove too detailed options in test launch files --- .../test/launch/swervebot.launch | 30 +------------------ .../test/launch/swervebot_control.launch | 1 - .../test/launch/swervebot_plot.launch | 6 ---- 3 files changed, 1 insertion(+), 36 deletions(-) delete mode 100644 swerve_controller/test/launch/swervebot_plot.launch diff --git a/swerve_controller/test/launch/swervebot.launch b/swerve_controller/test/launch/swervebot.launch index 854594408..d02cc912f 100644 --- a/swerve_controller/test/launch/swervebot.launch +++ b/swerve_controller/test/launch/swervebot.launch @@ -6,26 +6,10 @@ - - - - - - - - - - - - - - - - @@ -34,13 +18,11 @@ - ros + - - - - - - - - - - - diff --git a/swerve_controller/test/launch/swervebot_control.launch b/swerve_controller/test/launch/swervebot_control.launch index 46f55c50c..f2b58aa46 100644 --- a/swerve_controller/test/launch/swervebot_control.launch +++ b/swerve_controller/test/launch/swervebot_control.launch @@ -31,7 +31,6 @@ - diff --git a/swerve_controller/test/launch/swervebot_plot.launch b/swerve_controller/test/launch/swervebot_plot.launch deleted file mode 100644 index 73379be68..000000000 --- a/swerve_controller/test/launch/swervebot_plot.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - From 93537a71d78dca40a1b3db74ee0c81a8d9fa5800 Mon Sep 17 00:00:00 2001 From: Gabriel Urbain Date: Mon, 10 Aug 2020 16:05:35 +0200 Subject: [PATCH 6/6] bug fix for parsing config file instead of URDF for geometric parameters --- swerve_controller/src/swerve_controller.cpp | 70 ++++++++++++------- .../test/config/swervebot_control.yaml | 6 ++ .../test/launch/swervebot.launch | 2 + 3 files changed, 51 insertions(+), 27 deletions(-) diff --git a/swerve_controller/src/swerve_controller.cpp b/swerve_controller/src/swerve_controller.cpp index ebefa4b9a..420a861d6 100644 --- a/swerve_controller/src/swerve_controller.cpp +++ b/swerve_controller/src/swerve_controller.cpp @@ -151,34 +151,48 @@ namespace swerve_controller // Get robot physical parameters from URDF or parameter server bool lookup_track = !controller_nh.getParam("track", track_); + bool lookup_wheel_steering_y_offset = !controller_nh.getParam("wheel_steering_y_offset", + wheel_steering_y_offset_); bool lookup_wheel_radius = !controller_nh.getParam("wheel_radius", wheel_radius_); bool lookup_wheel_base = !controller_nh.getParam("wheel_base", wheel_base_); - urdf_geometry_parser::UrdfGeometryParser uvk(root_nh, base_frame_id_); - if (lookup_track) - if (!uvk.getDistanceBetweenJoints(lf_wheel_name, rf_wheel_name, track_)) - return false; - else - controller_nh.setParam("track", track_); - - if (!uvk.getDistanceBetweenJoints(lf_steering_name, lf_wheel_name, - wheel_steering_y_offset_)) - return false; - else - controller_nh.setParam("wheel_steering_y_offset", wheel_steering_y_offset_); - - if (lookup_wheel_radius) - if (!uvk.getJointRadius(lf_wheel_name, wheel_radius_)) - return false; - else - controller_nh.setParam("wheel_radius", wheel_radius_); - - if (lookup_wheel_base) - if (!uvk.getDistanceBetweenJoints(lf_wheel_name, lh_wheel_name, - wheel_base_)) - return false; - else - controller_nh.setParam("wheel_base", wheel_base_); + if (lookup_track || lookup_wheel_steering_y_offset || + lookup_wheel_radius || lookup_wheel_base) + { + ROS_INFO_STREAM("Some geometric parameters are not provided in the config file." + << "Parsing from URDF!"); + urdf_geometry_parser::UrdfGeometryParser uvk(root_nh, base_frame_id_); + if (lookup_track) + { + if (!uvk.getDistanceBetweenJoints(lf_wheel_name, rf_wheel_name, track_)) + return false; + else + controller_nh.setParam("track", track_); + } + if (lookup_wheel_steering_y_offset) + { + if (!uvk.getDistanceBetweenJoints(lf_steering_name, lf_wheel_name, + wheel_steering_y_offset_)) + return false; + else + controller_nh.setParam("wheel_steering_y_offset", wheel_steering_y_offset_); + } + if (lookup_wheel_radius) + { + if (!uvk.getJointRadius(lf_wheel_name, wheel_radius_)) + return false; + else + controller_nh.setParam("wheel_radius", wheel_radius_); + } + if (lookup_wheel_base) + { + if (!uvk.getDistanceBetweenJoints(lf_wheel_name, lh_wheel_name, + wheel_base_)) + return false; + else + controller_nh.setParam("wheel_base", wheel_base_); + } + } // Set physical parameters in odometry odometry_.setWheelParams(track_ - 2 * wheel_steering_y_offset_, wheel_radius_, wheel_base_); @@ -250,14 +264,16 @@ namespace swerve_controller const double rf_speed = rf_wheel_joint_->getVelocity(); const double lh_speed = lh_wheel_joint_->getVelocity(); const double rh_speed = rh_wheel_joint_->getVelocity(); - if (std::isnan(lf_speed) || std::isnan(rf_speed) || std::isnan(lh_speed) || std::isnan(rh_speed)) + if (std::isnan(lf_speed) || std::isnan(rf_speed) || + std::isnan(lh_speed) || std::isnan(rh_speed)) return; const double lf_steering = lf_steering_joint_->getPosition(); const double rf_steering = rf_steering_joint_->getPosition(); const double lh_steering = lh_steering_joint_->getPosition(); const double rh_steering = rh_steering_joint_->getPosition(); - if (std::isnan(lf_steering) || std::isnan(rf_steering) || std::isnan(lh_steering) || std::isnan(rh_steering)) + if (std::isnan(lf_steering) || std::isnan(rf_steering) || + std::isnan(lh_steering) || std::isnan(rh_steering)) return; // Estimate linear and angular velocity using joint information diff --git a/swerve_controller/test/config/swervebot_control.yaml b/swerve_controller/test/config/swervebot_control.yaml index 157b362ea..1d1edfb04 100644 --- a/swerve_controller/test/config/swervebot_control.yaml +++ b/swerve_controller/test/config/swervebot_control.yaml @@ -30,6 +30,12 @@ swervebot: cmd_vel_timeout: 5 base_frame_id: "body" + # Geometry (Do not provide the following if you want it to be parsed in the URDF) + # track: 0.8 + # wheel_steering_y_offset: 0 + # wheel_radius: 0.15 + # wheel_base: 1.2 + # Low-Level Controllers ------------------------------------- gazebo_ros_control: pid_gains: diff --git a/swerve_controller/test/launch/swervebot.launch b/swerve_controller/test/launch/swervebot.launch index d02cc912f..e801de4ae 100644 --- a/swerve_controller/test/launch/swervebot.launch +++ b/swerve_controller/test/launch/swervebot.launch @@ -23,6 +23,8 @@ +