diff --git a/swerve_controller/CMakeLists.txt b/swerve_controller/CMakeLists.txt new file mode 100644 index 000000000..207248c9b --- /dev/null +++ b/swerve_controller/CMakeLists.txt @@ -0,0 +1,56 @@ +cmake_minimum_required(VERSION 2.8.3) +project(swerve_controller) + +# Package dependencies +find_package(catkin REQUIRED COMPONENTS + controller_interface + joint_state_controller + nav_msgs + realtime_tools + robot_state_publisher + roslint + realtime_tools + tf + urdf_geometry_parser) + +# Create the package +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS ${${PROJECT_NAME}_CATKIN_DEPS} +) + +# 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}) + +# 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..15ce0b40a --- /dev/null +++ b/swerve_controller/include/swerve_controller/swerve_controller.h @@ -0,0 +1,178 @@ +/********************************************************************* + * 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(); + + 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 starting(const ros::Time &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_; + + /// Range of steering angle: + double min_steering_angle_, max_steering_angle_; + + /// 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: + void updateOdometry(const ros::Time &time); + + void updateCommand(const ros::Time &time, const ros::Duration &period); + + void brake(); + + bool clipSteeringAngle(double &steering, double &speed); + + void cmdVelCallback(const geometry_msgs::Twist &command); + + bool getPhysicalParams(ros::NodeHandle &controller_nh); + + 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..420a861d6 --- /dev/null +++ b/swerve_controller/src/swerve_controller.cpp @@ -0,0 +1,515 @@ +/********************************************************************* + * 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), + 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 &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 params !"); + 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 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_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_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_); + + 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_); + 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); + } + + // 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_) + { + 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_steering_joint_ && rf_steering_joint_ && lh_steering_joint_ && rh_steering_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); + } + } + + 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()) + { + 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/tf_ground_truth b/swerve_controller/test/bin/tf_ground_truth new file mode 100755 index 000000000..f8e995537 --- /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..1d1edfb04 --- /dev/null +++ b/swerve_controller/test/config/swervebot_control.yaml @@ -0,0 +1,49 @@ +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" + + # Range of motion of steering motors + min_steering_angle: -1.58 + max_steering_angle: 1.58 + + # 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" + + # 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: + 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/include/swerve_controller/swervebot.h b/swerve_controller/test/include/swerve_controller/swervebot.h new file mode 100644 index 000000000..364e85ea6 --- /dev/null +++ b/swerve_controller/test/include/swerve_controller/swervebot.h @@ -0,0 +1,204 @@ +/********************************************************************* + * 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.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: + /** + * \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..e801de4ae --- /dev/null +++ b/swerve_controller/test/launch/swervebot.launch @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/swerve_controller/test/launch/swervebot_control.launch b/swerve_controller/test/launch/swervebot_control.launch new file mode 100644 index 000000000..f2b58aa46 --- /dev/null +++ b/swerve_controller/test/launch/swervebot_control.launch @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + 100 + + + + + + + + + + + + + + + + + + + + + diff --git a/swerve_controller/test/launch/swervebot_rviz.launch b/swerve_controller/test/launch/swervebot_rviz.launch new file mode 100644 index 000000000..58d293afe --- /dev/null +++ b/swerve_controller/test/launch/swervebot_rviz.launch @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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..a93818da1 --- /dev/null +++ b/swerve_controller/test/urdf/swervebot.xacro @@ -0,0 +1,402 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 + + + +