diff --git a/mecanum_drive_controller/CHANGELOG.rst b/mecanum_drive_controller/CHANGELOG.rst new file mode 100644 index 000000000..e9e106ec3 --- /dev/null +++ b/mecanum_drive_controller/CHANGELOG.rst @@ -0,0 +1,7 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package mecanum_drive_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.1 (2014-10-27) +------------------ +* First version of the mecanum drive controller. diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt new file mode 100644 index 000000000..7b8429004 --- /dev/null +++ b/mecanum_drive_controller/CMakeLists.txt @@ -0,0 +1,43 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mecanum_drive_controller) + +find_package(catkin REQUIRED COMPONENTS + controller_interface + urdf + realtime_tools + tf + nav_msgs) + +################################################ +# List sources. +################################################ +set(${PROJECT_NAME}_headers + include/mecanum_drive_controller/mecanum_drive_controller.h + include/mecanum_drive_controller/odometry.h +) + +set(${PROJECT_NAME}_sources + src/mecanum_drive_controller.cpp + src/odometry.cpp +) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} +) + +include_directories( + include ${catkin_INCLUDE_DIRS} +) + +add_library(${PROJECT_NAME} ${${PROJECT_NAME}_headers} ${${PROJECT_NAME}_sources}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + +install(FILES mecanum_drive_controller_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/mecanum_drive_controller/README.md b/mecanum_drive_controller/README.md new file mode 100644 index 000000000..40f80820a --- /dev/null +++ b/mecanum_drive_controller/README.md @@ -0,0 +1,33 @@ +--- MECANUM DRIVE CONTROLLER --- + + +INTRODUCTION + * Controller for a mecanum drive mobile base + * This is a 2D planar controller + * The development of this controller was based on the differential drive controller called diff_drive_controller + +RESTRICTIONS and ASSUMPTIONS + * the controller only works with 4WD mobiles + * the mobile robot is associated a frame called the CENTER frame + - the origin is at the center of the wheels' contact points + - X is front + - Y is left + * the 4 wheels are identical in radius + * the wheels rotation axis is aligned with Y (not -Y) + - that is the axis of rotation of each wheel is in the Y direction + * the wheels need to have the same parent frame + * the projection of the wheels frame origin on the floor is the contact point of the wheel + +DESCRIPTION + * The velocity command is a body velocity, this body frame is called the BASE frame + - By default the BASE frame = the CENTER frame + * The BASE frame can be offset (with respect to the CENTER frame) using the **base_frame_offset** parameter + - WARNING: the offset of the BASE frame with respect to the CENTER is **not automatically retrieved**, it needs to be indicated by hand + * The odometry computes the location of a frame which is also the BASE frame + - The reference frame of the odometry is the initial position of the BASE frame at t=0 + - This reference frame is called the ODOMETRY frame + * The wheels are refered to using an index 0, 1, 2, 3 + - wheel 0: front left wheel + - wheel 1: back left wheel + - wheel 2: back right wheel + - wheel 3: front right wheel \ No newline at end of file diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h new file mode 100644 index 000000000..c340a78c2 --- /dev/null +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h @@ -0,0 +1,148 @@ +#pragma once + +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include + +namespace mecanum_drive_controller +{ +// Check file README.md for restrictions and notes. +class MecanumDriveController : public controller_interface::Controller +{ +public: + MecanumDriveController(); + + /** + * \brief Initialize controller + * \param hw Velocity joint interface for the wheels + * \param root_nh Node handle at root namespace + * \param controller_nh Node handle inside the controller namespace + */ + bool init(hardware_interface::VelocityJointInterface* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh); + + /** + * \brief Updates controller, i.e. computes the odometry and sets the new velocity commands + * \param time Current time + * \param period Time since the last called to update + */ + void update(const ros::Time& time, const ros::Duration& period); + + /** + * \brief Starts controller + * \param time Current time + */ + void starting(const ros::Time& time); + + /** + * \brief Stops controller + * \param time Current time + */ + void stopping(const ros::Time& time); + +private: + std::string name_; + + /// Hardware handles: + hardware_interface::JointHandle wheel0_jointHandle_; + hardware_interface::JointHandle wheel1_jointHandle_; + hardware_interface::JointHandle wheel2_jointHandle_; + hardware_interface::JointHandle wheel3_jointHandle_; + + /// Velocity command related: + struct Command + { + double vx_Ob_b_b0_b; + double vy_Ob_b_b0_b; + double wz_b_b0_b; + ros::Time stamp; + + Command() : vx_Ob_b_b0_b(0.0), vy_Ob_b_b0_b(0.0), wz_b_b0_b(0.0), stamp(0.0) + { + } + }; + realtime_tools::RealtimeBuffer command_rt_buffer_; + Command command_; + ros::Subscriber command_sub_; + + /// Odometry related: + Odometry odometry_; + + std::shared_ptr > odom_pub_; + std::shared_ptr > tf_pub_; + + geometry_msgs::TransformStamped odom_frame_; + + ros::Duration publish_period_; + ros::Time last_state_publish_time_; + + /// Wheel radius (assuming it's the same for the left and right wheels): + bool use_realigned_roller_joints_; + double wheels_k_; // wheels geometric param used in mecanum wheels' ik + double wheels_radius_; + + /// Timeout to consider cmd_vel commands old: + double cmd_vel_timeout_; + + /// Frame to use for the robot base: + std::string base_frame_id_; + double base_frame_offset_[PLANAR_POINT_DIM]; + + /// Whether to publish odometry to tf or not: + bool enable_odom_tf_; + + /// Number of wheel joints: + size_t wheel_joints_size_; + +private: + /** + * \brief Brakes the wheels, i.e. sets the velocity to 0 + */ + void brake(); + + /** + * \brief Velocity command callback + * \param command Velocity command message (twist) + */ + void commandCb(const geometry_msgs::Twist& command); + + /** + * \brief Sets odometry parameters from the URDF, i.e. the wheel radius and separation + * \param root_nh Root node handle + * \param wheel0_name Name of wheel0 joint + * \param wheel1_name Name of wheel1 joint + * \param wheel2_name Name of wheel2 joint + * \param wheel3_name Name of wheel3 joint + */ + bool setWheelParamsFromUrdf(ros::NodeHandle& root_nh, const std::string& wheel0_name, const std::string& wheel1_name, + const std::string& wheel2_name, const std::string& wheel3_name); + + /** + * \brief Get the radius of a given wheel + * \param model urdf model used + * \param wheel_link link of the wheel from which to get the radius + * \param[out] wheels_radius radius of the wheel read from the urdf + */ + bool getWheelRadius(const urdf::ModelInterfaceSharedPtr& model, const urdf::LinkConstSharedPtr& wheel_link, + double& wheel_radius); + + /** + * \brief Sets the odometry publishing fields + * \param root_nh Root node handle + * \param controller_nh Node handle inside the controller namespace + */ + void setupRealtimePublishersMsg(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh); +}; + +PLUGINLIB_EXPORT_CLASS(mecanum_drive_controller::MecanumDriveController, controller_interface::ControllerBase) + +} // namespace mecanum_drive_controller diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h new file mode 100644 index 000000000..0e1a469b4 --- /dev/null +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.h @@ -0,0 +1,114 @@ +#ifndef ODOMETRY_H_ +#define ODOMETRY_H_ + +#include +#include +#include +#include +#include + +#define PLANAR_POINT_DIM 3 + +namespace mecanum_drive_controller +{ +namespace bacc = boost::accumulators; + +/// \brief The Odometry class handles odometry readings +/// (2D pose and velocity with related timestamp) +class Odometry +{ +public: + /// Integration function, used to integrate the odometry: + typedef boost::function IntegrationFunction; + + /// \brief Constructor + /// Timestamp will get the current time value + /// Value will be set to zero + /// \param velocity_rolling_window_size Rolling window size used to compute the velocity mean + Odometry(size_t velocity_rolling_window_size = 10); + + /// \brief Initialize the odometry + /// \param time Current time + void init(const ros::Time& time, double base_frame_offset[PLANAR_POINT_DIM]); + + /// \brief Updates the odometry class with latest wheels position + /// \param wheel0_vel Wheel velocity [rad/s] + /// \param wheel1_vel Wheel velocity [rad/s] + /// \param wheel2_vel Wheel velocity [rad/s] + /// \param wheel3_vel Wheel velocity [rad/s] + /// \param time Current time + /// \return true if the odometry is actually updated + bool update(double wheel0_vel, double wheel1_vel, double wheel2_vel, double wheel3_vel, const ros::Time& time); + + /// \return position (x component) [m] + double getX() const + { + return px_b_b0_; + } + /// \return position (y component) [m] + double getY() const + { + return py_b_b0_; + } + /// \return orientation (z component) [m] + double getRz() const + { + return rz_b_b0_; + } + /// \return body velocity of the base frame (linear x component) [m/s] + double getVx() const + { + return vx_Ob_b_b0_b_; + } + /// \return body velocity of the base frame (linear y component) [m/s] + double getVy() const + { + return vy_Ob_b_b0_b_; + } + /// \return body velocity of the base frame (angular z component) [m/s] + double getWz() const + { + return wz_b_b0_b_; + } + + /// \brief Sets the wheels parameters: mecanum geometric param and radius + /// \param wheels_k Wheels geometric param (used in mecanum wheels' ik) [m] + /// \param wheels_radius Wheels radius [m] + void setWheelsParams(double wheels_k, double wheels_radius); + +private: + /// Rolling mean accumulator and window: + typedef bacc::accumulator_set > RollingMeanAcc; + typedef bacc::tag::rolling_window RollingWindow; + + /// Current timestamp: + ros::Time timestamp_; + + /// Reference frame (wrt to center frame). + double base_frame_offset_[PLANAR_POINT_DIM]; + + /// Current pose: + double px_b_b0_; // [m] + double py_b_b0_; // [m] + double rz_b_b0_; // [rad] + + /// Current velocity. + /// \note The indices meaning is the following: + /// b : base frame + /// c : center frame + /// Ob: origin of the base frame + /// Oc: origin of the center frame + /// b0: initial position if the base frame + /// c0: initial position of the center frame + double vx_Ob_b_b0_b_; // [m/s] + double vy_Ob_b_b0_b_; // [m/s] + double wz_b_b0_b_; // [rad/s] + + /// Wheels kinematic parameters [m]: + double wheels_k_; + double wheels_radius_; +}; + +} // namespace mecanum_drive_controller + +#endif /* ODOMETRY_H_ */ diff --git a/mecanum_drive_controller/mecanum_drive_controller_plugins.xml b/mecanum_drive_controller/mecanum_drive_controller_plugins.xml new file mode 100644 index 000000000..78e1b277e --- /dev/null +++ b/mecanum_drive_controller/mecanum_drive_controller_plugins.xml @@ -0,0 +1,9 @@ + + + + + The MecanumDriveController tracks velocity commands. It 4 joints should have a VelocityJointInterface hardware interface. + + + + diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml new file mode 100644 index 000000000..5e3394924 --- /dev/null +++ b/mecanum_drive_controller/package.xml @@ -0,0 +1,28 @@ + + mecanum_drive_controller + 0.0.1 + Controller for a mecanum drive mobile base. + Antoine Rennuit + + MIT + + Antoine Rennuit + + catkin + + controller_interface + nav_msgs + realtime_tools + tf + urdf + + controller_interface + nav_msgs + realtime_tools + tf + urdf + + + + + diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp new file mode 100644 index 000000000..98fecd609 --- /dev/null +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -0,0 +1,438 @@ +#include +#include +#include + +static bool isCylinderOrSphere(const urdf::LinkConstSharedPtr& link) +{ + if (!link) + { + ROS_ERROR("Link == NULL."); + return false; + } + + if (!link->collision) + { + ROS_ERROR_STREAM("Link " << link->name + << " does not have collision description. Add collision description for link to urdf."); + return false; + } + + if (!link->collision->geometry) + { + ROS_ERROR_STREAM("Link " << link->name + << " does not have collision geometry description. Add collision geometry description for " + "link to urdf."); + return false; + } + + if (link->collision->geometry->type != urdf::Geometry::CYLINDER && + link->collision->geometry->type != urdf::Geometry::SPHERE) + { + ROS_ERROR_STREAM("Link " << link->name << " does not have cylinder nor sphere geometry"); + return false; + } + + return true; +} + +namespace mecanum_drive_controller +{ +MecanumDriveController::MecanumDriveController() + : command_() + , use_realigned_roller_joints_(false) + , wheels_k_(0.0) + , wheels_radius_(0.0) + , cmd_vel_timeout_(0.5) + , base_frame_id_("base_link") + , base_frame_offset_{ 0.0, 0.0, 0.0 } + , enable_odom_tf_(true) + , wheel_joints_size_(0) +{ +} + +bool MecanumDriveController::init(hardware_interface::VelocityJointInterface* hw, ros::NodeHandle& root_nh, + ros::NodeHandle& controller_nh) +{ + const std::string complete_ns = controller_nh.getNamespace(); + std::size_t id = complete_ns.find_last_of("/"); + name_ = complete_ns.substr(id + 1); + + // Option use_realigned_roller_joints + controller_nh.param("use_realigned_roller_joints", use_realigned_roller_joints_, use_realigned_roller_joints_); + ROS_INFO_STREAM_NAMED(name_, "Use the roller's radius rather than the wheel's: " + << (use_realigned_roller_joints_ ? "true" : "false") << "."); + + // Get joint names from the parameter server + std::string wheel0_name; + controller_nh.param("front_left_wheel_joint", wheel0_name, wheel0_name); + ROS_INFO_STREAM_NAMED(name_, "Front left wheel joint (wheel0) is : " << wheel0_name); + + std::string wheel1_name; + controller_nh.param("back_left_wheel_joint", wheel1_name, wheel1_name); + ROS_INFO_STREAM_NAMED(name_, "Back left wheel joint (wheel1) is : " << wheel1_name); + + std::string wheel2_name; + controller_nh.param("back_right_wheel_joint", wheel2_name, wheel2_name); + ROS_INFO_STREAM_NAMED(name_, "Back right wheel joint (wheel2) is : " << wheel2_name); + + std::string wheel3_name; + controller_nh.param("front_right_wheel_joint", wheel3_name, wheel3_name); + ROS_INFO_STREAM_NAMED(name_, "Front right wheel joint (wheel3) is : " << wheel3_name); + + // Odometry related: + double publish_rate; + controller_nh.param("publish_rate", publish_rate, 50.0); + ROS_INFO_STREAM_NAMED(name_, "Controller state published frequency : " << publish_rate << "Hz."); + publish_period_ = ros::Duration(1.0 / publish_rate); + + // Twist command related: + controller_nh.param("cmd_vel_timeout", cmd_vel_timeout_, cmd_vel_timeout_); + ROS_INFO_STREAM_NAMED(name_, + "Velocity commands will be considered old if they are older than " << cmd_vel_timeout_ << "s."); + + controller_nh.param("base_frame_id", base_frame_id_, base_frame_id_); + ROS_INFO_STREAM_NAMED(name_, "Base frame_id : " << base_frame_id_); + + // TODO: automatically compute the offset of the base frame with respect to the center frame. + controller_nh.param("base_frame_offset/x", base_frame_offset_[0], base_frame_offset_[0]); + controller_nh.param("base_frame_offset/y", base_frame_offset_[1], base_frame_offset_[1]); + controller_nh.param("base_frame_offset/theta", base_frame_offset_[2], base_frame_offset_[2]); + ROS_INFO_STREAM_NAMED(name_, "base_frame_offset : " << base_frame_offset_[0] << " " << base_frame_offset_[1] << " " + << base_frame_offset_[2]); + + controller_nh.param("enable_odom_tf", enable_odom_tf_, enable_odom_tf_); + ROS_INFO_STREAM_NAMED(name_, "Publishing to tf : " << (enable_odom_tf_ ? "enabled" : "disabled")); + + // Get the joint objects to use in the realtime loop + wheel0_jointHandle_ = hw->getHandle(wheel0_name); // throws on failure + wheel1_jointHandle_ = hw->getHandle(wheel1_name); // throws on failure + wheel2_jointHandle_ = hw->getHandle(wheel2_name); // throws on failure + wheel3_jointHandle_ = hw->getHandle(wheel3_name); // throws on failure + + // Pass params through and setup publishers and subscribers + if (!setWheelParamsFromUrdf(root_nh, wheel0_name, wheel1_name, wheel2_name, wheel3_name)) + return false; + + setupRealtimePublishersMsg(root_nh, controller_nh); + + command_sub_ = controller_nh.subscribe("cmd_vel", 1, &MecanumDriveController::commandCb, this); + + return true; +} + +void MecanumDriveController::update(const ros::Time& time, const ros::Duration& period) +{ + // FORWARD KINEMATICS (odometry). + double wheel0_vel = wheel0_jointHandle_.getVelocity(); + double wheel1_vel = wheel1_jointHandle_.getVelocity(); + double wheel2_vel = wheel2_jointHandle_.getVelocity(); + double wheel3_vel = wheel3_jointHandle_.getVelocity(); + + if (std::isnan(wheel0_vel) || std::isnan(wheel1_vel) || std::isnan(wheel2_vel) || std::isnan(wheel3_vel)) + return; + + // Estimate twist (using joint information) and integrate + odometry_.update(wheel0_vel, wheel1_vel, wheel2_vel, wheel3_vel, time); + + // Publish odometry message + if (last_state_publish_time_ + publish_period_ < time) + { + last_state_publish_time_ += publish_period_; + + // Compute and store orientation info + const geometry_msgs::Quaternion orientation(tf::createQuaternionMsgFromYaw(odometry_.getRz())); + + // 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_.getVx(); + odom_pub_->msg_.twist.twist.linear.y = odometry_.getVy(); + odom_pub_->msg_.twist.twist.angular.z = odometry_.getWz(); + + odom_pub_->unlockAndPublish(); + } + + // Publish tf /odom frame + if (enable_odom_tf_ && tf_pub_->trylock()) + { + geometry_msgs::TransformStamped& odom_frame = tf_pub_->msg_.transforms[0]; + odom_frame.header.stamp = time; + odom_frame.transform.translation.x = odometry_.getX(); + odom_frame.transform.translation.y = odometry_.getY(); + odom_frame.transform.rotation = orientation; + + tf_pub_->unlockAndPublish(); + } + } + + // INVERSE KINEMATICS (move robot). + + // Brake if cmd_vel has timeout: + Command curr_cmd = *(command_rt_buffer_.readFromRT()); + const double dt = (time - curr_cmd.stamp).toSec(); + + if (dt > cmd_vel_timeout_) + { + curr_cmd.vx_Ob_b_b0_b = 0.0; + curr_cmd.vy_Ob_b_b0_b = 0.0; + curr_cmd.wz_b_b0_b = 0.0; + } + + // Compute wheels velocities (this is the actual ik): + // NOTE: the input desired twist (from topic /cmd_vel) is a body twist. + tf::Matrix3x3 R_b_c = tf::Matrix3x3(tf::createQuaternionFromYaw(base_frame_offset_[2])); + tf::Vector3 v_Ob_b_b0_c = R_b_c * tf::Vector3(curr_cmd.vx_Ob_b_b0_b, curr_cmd.vy_Ob_b_b0_b, 0.0); + tf::Vector3 Ob_c = tf::Vector3(base_frame_offset_[0], base_frame_offset_[1], 0.0); + + double vx_Oc_c_c0_c_ = v_Ob_b_b0_c.x() + Ob_c.y() * curr_cmd.wz_b_b0_b; + double vy_Oc_c_c0_c_ = v_Ob_b_b0_c.y() - Ob_c.x() * curr_cmd.wz_b_b0_b; + double wz_c_c0_c_ = curr_cmd.wz_b_b0_b; + + double w0_vel = 1.0 / wheels_radius_ * (vx_Oc_c_c0_c_ - vy_Oc_c_c0_c_ - wheels_k_ * wz_c_c0_c_); + double w1_vel = 1.0 / wheels_radius_ * (vx_Oc_c_c0_c_ + vy_Oc_c_c0_c_ - wheels_k_ * wz_c_c0_c_); + double w2_vel = 1.0 / wheels_radius_ * (vx_Oc_c_c0_c_ - vy_Oc_c_c0_c_ + wheels_k_ * wz_c_c0_c_); + double w3_vel = 1.0 / wheels_radius_ * (vx_Oc_c_c0_c_ + vy_Oc_c_c0_c_ + wheels_k_ * wz_c_c0_c_); + + // Set wheels velocities: + wheel0_jointHandle_.setCommand(w0_vel); + wheel1_jointHandle_.setCommand(w1_vel); + wheel2_jointHandle_.setCommand(w2_vel); + wheel3_jointHandle_.setCommand(w3_vel); +} + +void MecanumDriveController::starting(const ros::Time& time) +{ + brake(); + + // Register starting time used to keep fixed rate + last_state_publish_time_ = time; + + odometry_.init(time, base_frame_offset_); +} + +void MecanumDriveController::stopping(const ros::Time& time) +{ + brake(); +} + +void MecanumDriveController::brake() +{ + wheel0_jointHandle_.setCommand(0.0); + wheel1_jointHandle_.setCommand(0.0); + wheel2_jointHandle_.setCommand(0.0); + wheel3_jointHandle_.setCommand(0.0); +} + +void MecanumDriveController::commandCb(const geometry_msgs::Twist& command) +{ + if (!isRunning()) + { + ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running."); + return; + } + + command_.vx_Ob_b_b0_b = command.linear.x; + command_.vy_Ob_b_b0_b = command.linear.y; + command_.wz_b_b0_b = command.angular.z; + + command_.stamp = ros::Time::now(); + command_rt_buffer_.writeFromNonRT(command_); + + ROS_DEBUG_STREAM_NAMED(name_, "Added values to command. " + << "vx_Ob_b_b0_b : " << command_.vx_Ob_b_b0_b << ", " + << "vy_Ob_b_b0_b : " << command_.vy_Ob_b_b0_b << ", " + << "wz_b_b0_b : " << command_.wz_b_b0_b << ", " + << "Stamp : " << command_.stamp); +} + +bool MecanumDriveController::setWheelParamsFromUrdf(ros::NodeHandle& root_nh, const std::string& wheel0_name, + const std::string& wheel1_name, const std::string& wheel2_name, + const std::string& wheel3_name) +{ + // Parse robot description + const std::string model_param_name = "robot_description"; + bool res = root_nh.hasParam(model_param_name); + std::string robot_model_str = ""; + if (!res || !root_nh.getParam(model_param_name, robot_model_str)) + { + ROS_ERROR_NAMED(name_, "Robot descripion couldn't be retrieved from param server."); + return false; + } + + urdf::ModelInterfaceSharedPtr model(urdf::parseURDF(robot_model_str)); + + // Get wheels position and compute parameter k_ (used in mecanum wheels IK). + urdf::JointConstSharedPtr wheel0_urdfJoint(model->getJoint(wheel0_name)); + if (!wheel0_urdfJoint) + { + ROS_ERROR_STREAM_NAMED(name_, wheel0_name << " couldn't be retrieved from model description"); + return false; + } + urdf::JointConstSharedPtr wheel1_urdfJoint(model->getJoint(wheel1_name)); + if (!wheel1_urdfJoint) + { + ROS_ERROR_STREAM_NAMED(name_, wheel1_name << " couldn't be retrieved from model description"); + return false; + } + urdf::JointConstSharedPtr wheel2_urdfJoint(model->getJoint(wheel2_name)); + if (!wheel2_urdfJoint) + { + ROS_ERROR_STREAM_NAMED(name_, wheel2_name << " couldn't be retrieved from model description"); + return false; + } + urdf::JointConstSharedPtr wheel3_urdfJoint(model->getJoint(wheel3_name)); + if (!wheel3_urdfJoint) + { + ROS_ERROR_STREAM_NAMED(name_, wheel3_name << " couldn't be retrieved from model description"); + return false; + } + + ROS_INFO_STREAM("wheel0 to origin: " << wheel0_urdfJoint->parent_to_joint_origin_transform.position.x << "," + << wheel0_urdfJoint->parent_to_joint_origin_transform.position.y << ", " + << wheel0_urdfJoint->parent_to_joint_origin_transform.position.z); + ROS_INFO_STREAM("wheel1 to origin: " << wheel1_urdfJoint->parent_to_joint_origin_transform.position.x << "," + << wheel1_urdfJoint->parent_to_joint_origin_transform.position.y << ", " + << wheel1_urdfJoint->parent_to_joint_origin_transform.position.z); + ROS_INFO_STREAM("wheel2 to origin: " << wheel2_urdfJoint->parent_to_joint_origin_transform.position.x << "," + << wheel2_urdfJoint->parent_to_joint_origin_transform.position.y << ", " + << wheel2_urdfJoint->parent_to_joint_origin_transform.position.z); + ROS_INFO_STREAM("wheel3 to origin: " << wheel3_urdfJoint->parent_to_joint_origin_transform.position.x << "," + << wheel3_urdfJoint->parent_to_joint_origin_transform.position.y << ", " + << wheel3_urdfJoint->parent_to_joint_origin_transform.position.z); + + double wheel0_x = wheel0_urdfJoint->parent_to_joint_origin_transform.position.x; + double wheel0_y = wheel0_urdfJoint->parent_to_joint_origin_transform.position.y; + double wheel1_x = wheel1_urdfJoint->parent_to_joint_origin_transform.position.x; + double wheel1_y = wheel1_urdfJoint->parent_to_joint_origin_transform.position.y; + double wheel2_x = wheel2_urdfJoint->parent_to_joint_origin_transform.position.x; + double wheel2_y = wheel2_urdfJoint->parent_to_joint_origin_transform.position.y; + double wheel3_x = wheel3_urdfJoint->parent_to_joint_origin_transform.position.x; + double wheel3_y = wheel3_urdfJoint->parent_to_joint_origin_transform.position.y; + + wheels_k_ = (-(-wheel0_x - wheel0_y) - (wheel1_x - wheel1_y) + (-wheel2_x - wheel2_y) + (wheel3_x - wheel3_y)) / 4.0; + + // Get wheels radius + double wheel0_radius = 0.0; + double wheel1_radius = 0.0; + double wheel2_radius = 0.0; + double wheel3_radius = 0.0; + + if (!getWheelRadius(model, model->getLink(wheel0_urdfJoint->child_link_name), wheel0_radius) || + !getWheelRadius(model, model->getLink(wheel1_urdfJoint->child_link_name), wheel1_radius) || + !getWheelRadius(model, model->getLink(wheel2_urdfJoint->child_link_name), wheel2_radius) || + !getWheelRadius(model, model->getLink(wheel3_urdfJoint->child_link_name), wheel3_radius)) + { + ROS_ERROR_STREAM_NAMED(name_, "Couldn't retrieve wheels' radius"); + return false; + } + + if (fabs(wheel0_radius - wheel1_radius) > 1e-3 || fabs(wheel0_radius - wheel2_radius) > 1e-3 || + fabs(wheel0_radius - wheel3_radius) > 1e-3) + { + ROS_ERROR_STREAM_NAMED(name_, "Wheels radius are not egual"); + return false; + } + + wheels_radius_ = wheel0_radius; + + ROS_INFO_STREAM("Wheel radius: " << wheels_radius_); + + // Set wheel params for the odometry computation + odometry_.setWheelsParams(wheels_k_, wheels_radius_); + + return true; +} + +bool MecanumDriveController::getWheelRadius(const urdf::ModelInterfaceSharedPtr& model, + const urdf::LinkConstSharedPtr& wheel_link, double& wheel_radius) +{ + auto radius_link = wheel_link; + + if (use_realigned_roller_joints_) + { + // This mode is used when the mecanum wheels are simulated and we use realigned rollers to mimic mecanum wheels. + const auto& roller_joint = radius_link->child_joints[0]; + if (!roller_joint) + { + ROS_ERROR_STREAM_NAMED(name_, "No roller joint could be retrieved for wheel : " + << wheel_link->name + << ". Are you sure mecanum wheels are simulated using realigned rollers?"); + return false; + } + + radius_link = model->getLink(roller_joint->child_link_name); + if (!radius_link) + { + ROS_ERROR_STREAM_NAMED(name_, "No roller link could be retrieved for wheel : " + << wheel_link->name + << ". Are you sure mecanum wheels are simulated using realigned rollers?"); + return false; + } + } + + if (!isCylinderOrSphere(radius_link)) + { + ROS_ERROR_STREAM("Wheel link " << radius_link->name << " is NOT modeled as a cylinder!"); + return false; + } + + if (radius_link->collision->geometry->type == urdf::Geometry::CYLINDER) + wheel_radius = (static_cast(radius_link->collision->geometry.get()))->radius; + else + wheel_radius = (static_cast(radius_link->collision->geometry.get()))->radius; + + return true; +} + +void MecanumDriveController::setupRealtimePublishersMsg(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) +{ + // Get covariance parameters for odometry. + 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 msg. + odom_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "odomTopic", 100)); + odom_pub_->msg_.header.frame_id = "odom"; + odom_pub_->msg_.child_frame_id = base_frame_id_; + odom_pub_->msg_.pose.pose.position.z = 0; + 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]) }; + + // Setup tf msg. + tf_pub_.reset(new realtime_tools::RealtimePublisher(root_nh, "/tf", 100)); + tf_pub_->msg_.transforms.resize(1); + tf_pub_->msg_.transforms[0].transform.translation.z = 0.0; + tf_pub_->msg_.transforms[0].child_frame_id = base_frame_id_; + tf_pub_->msg_.transforms[0].header.frame_id = "odom"; +} + +} // namespace mecanum_drive_controller diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp new file mode 100644 index 000000000..574b20ad7 --- /dev/null +++ b/mecanum_drive_controller/src/odometry.cpp @@ -0,0 +1,82 @@ +#include + +#include + +#include + +namespace mecanum_drive_controller +{ +namespace bacc = boost::accumulators; + +Odometry::Odometry(size_t velocity_rolling_window_size) + : timestamp_(0.0) + , px_b_b0_(0.0) + , py_b_b0_(0.0) + , rz_b_b0_(0.0) + , vx_Ob_b_b0_b_(0.0) + , vy_Ob_b_b0_b_(0.0) + , wz_b_b0_b_(0.0) + , wheels_k_(0.0) + , wheels_radius_(0.0) +{ +} + +void Odometry::init(const ros::Time& time, double base_frame_offset[PLANAR_POINT_DIM]) +{ + // Reset timestamp: + timestamp_ = time; + + // Base frame offset (wrt to center frame). + base_frame_offset_[0] = base_frame_offset[0]; + base_frame_offset_[1] = base_frame_offset[1]; + base_frame_offset_[2] = base_frame_offset[2]; +} + +bool Odometry::update(double wheel0_vel, double wheel1_vel, double wheel2_vel, double wheel3_vel, const ros::Time& time) +{ + /// We cannot estimate the speed with very small time intervals: + const double dt = (time - timestamp_).toSec(); + if (dt < 0.0001) + return false; // Interval too small to integrate with + + timestamp_ = time; + + /// Compute FK (i.e. compute mobile robot's body twist out of its wheels velocities): + /// NOTE: we use the IK of the mecanum wheels which we invert using a pseudo-inverse. + /// NOTE: the mecanum IK gives the body speed at the center frame, we then offset this velocity at the base frame. + /// NOTE: in the diff drive the velocity is filtered out, but we prefer to return it raw and let the user perform + /// post-processing at will. We prefer this way of doing as filtering introduces delay (which makes it + /// difficult to interpret and compare behavior curves). + double vx_Oc_c_c0_c = 0.25 * wheels_radius_ * (wheel0_vel + wheel1_vel + wheel2_vel + wheel3_vel); + double vy_Oc_c_c0_c = 0.25 * wheels_radius_ * (-wheel0_vel + wheel1_vel - wheel2_vel + wheel3_vel); + double wz_c_c0_c = 0.25 * wheels_radius_ / wheels_k_ * (-wheel0_vel - wheel1_vel + wheel2_vel + wheel3_vel); + + tf::Matrix3x3 R_c_b = tf::Matrix3x3(tf::createQuaternionFromYaw(-base_frame_offset_[2])); + tf::Vector3 v_Oc_c_c0_b = R_c_b * tf::Vector3(vx_Oc_c_c0_c, vy_Oc_c_c0_c, 0.0); + tf::Vector3 Oc_b = R_c_b * tf::Vector3(-base_frame_offset_[0], -base_frame_offset_[1], 0.0); + + vx_Ob_b_b0_b_ = v_Oc_c_c0_b.x() + Oc_b.y() * wz_c_c0_c; + vy_Ob_b_b0_b_ = v_Oc_c_c0_b.y() - Oc_b.x() * wz_c_c0_c; + wz_b_b0_b_ = wz_c_c0_c; + + /// Integration. + /// NOTE: the position is expressed in the odometry frame (frame b0), unlike the twist which is expressed in the body + /// frame (frame b). + rz_b_b0_ += wz_b_b0_b_ * dt; + + tf::Matrix3x3 R_b_b0 = tf::Matrix3x3(tf::createQuaternionFromYaw(rz_b_b0_)); + tf::Vector3 vx_Ob_b_b0_b0 = R_b_b0 * tf::Vector3(vx_Ob_b_b0_b_, vy_Ob_b_b0_b_, 0.0); + + px_b_b0_ += vx_Ob_b_b0_b0.x() * dt; + py_b_b0_ += vx_Ob_b_b0_b0.y() * dt; + + return true; +} + +void Odometry::setWheelsParams(double wheels_k, double wheels_radius) +{ + wheels_k_ = wheels_k; + wheels_radius_ = wheels_radius; +} + +} // namespace mecanum_drive_controller diff --git a/mecanum_drive_controller/test/diff_drive_bad_urdf.test b/mecanum_drive_controller/test/diff_drive_bad_urdf.test new file mode 100644 index 000000000..2932c6f6b --- /dev/null +++ b/mecanum_drive_controller/test/diff_drive_bad_urdf.test @@ -0,0 +1,17 @@ + + + + + + + + + + + + + diff --git a/mecanum_drive_controller/test/diff_drive_common.launch b/mecanum_drive_controller/test/diff_drive_common.launch new file mode 100644 index 000000000..b63aaa8d6 --- /dev/null +++ b/mecanum_drive_controller/test/diff_drive_common.launch @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + diff --git a/mecanum_drive_controller/test/diff_drive_controller.test b/mecanum_drive_controller/test/diff_drive_controller.test new file mode 100644 index 000000000..ceaaa46fd --- /dev/null +++ b/mecanum_drive_controller/test/diff_drive_controller.test @@ -0,0 +1,13 @@ + + + + + + + + + + diff --git a/mecanum_drive_controller/test/diff_drive_controller_limits.test b/mecanum_drive_controller/test/diff_drive_controller_limits.test new file mode 100644 index 000000000..42d6bcb09 --- /dev/null +++ b/mecanum_drive_controller/test/diff_drive_controller_limits.test @@ -0,0 +1,16 @@ + + + + + + + + + + + + + diff --git a/mecanum_drive_controller/test/diff_drive_fail_test.cpp b/mecanum_drive_controller/test/diff_drive_fail_test.cpp new file mode 100644 index 000000000..f12048800 --- /dev/null +++ b/mecanum_drive_controller/test/diff_drive_fail_test.cpp @@ -0,0 +1,57 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2013, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Paul Mathieu + +#include "test_common.h" + +// TEST CASES +TEST_F(DiffDriveControllerTest, testWrongJointName) +{ + // the controller should never be alive + int secs = 0; + while(!isControllerAlive() && secs < 5) + { + ros::Duration(1.0).sleep(); + secs++; + } + // give up and assume controller load failure after 5 secondds + EXPECT_GE(secs, 5); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "diff_drive_fail_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/mecanum_drive_controller/test/diff_drive_limits_test.cpp b/mecanum_drive_controller/test/diff_drive_limits_test.cpp new file mode 100644 index 000000000..888ef5eb6 --- /dev/null +++ b/mecanum_drive_controller/test/diff_drive_limits_test.cpp @@ -0,0 +1,169 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2013, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Paul Mathieu + +#include "test_common.h" + +// TEST CASES +TEST_F(DiffDriveControllerTest, testLinearAccelerationLimits) +{ + // wait for ROS + while(!isControllerAlive()) + { + ros::Duration(0.1).sleep(); + } + // zero everything before test + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.angular.z = 0.0; + publish(cmd_vel); + ros::Duration(2.0).sleep(); + // get initial odom + nav_msgs::Odometry old_odom = getLastOdom(); + // send a big command + cmd_vel.linear.x = 10.0; + publish(cmd_vel); + // wait for a while + ros::Duration(0.5).sleep(); + + nav_msgs::Odometry new_odom = getLastOdom(); + + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), 0.5 + VELOCITY_TOLERANCE); + EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), EPS); + + cmd_vel.linear.x = 0.0; + publish(cmd_vel); +} + +TEST_F(DiffDriveControllerTest, testLinearVelocityLimits) +{ + // wait for ROS + while(!isControllerAlive()) + { + ros::Duration(0.1).sleep(); + } + // zero everything before test + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.angular.z = 0.0; + publish(cmd_vel); + ros::Duration(2.0).sleep(); + // get initial odom + nav_msgs::Odometry old_odom = getLastOdom(); + // send a big command + cmd_vel.linear.x = 10.0; + publish(cmd_vel); + // wait for a while + ros::Duration(5.0).sleep(); + + nav_msgs::Odometry new_odom = getLastOdom(); + + // check if the robot speed is now 1.0 m.s-1, the limit + EXPECT_LT(new_odom.pose.pose.position.x - old_odom.pose.pose.position.x, 5.0 + POSITION_TOLERANCE); + EXPECT_LT(fabs(new_odom.pose.pose.position.y - old_odom.pose.pose.position.y), EPS); + + cmd_vel.linear.x = 0.0; + publish(cmd_vel); +} + +TEST_F(DiffDriveControllerTest, testAngularAccelerationLimits) +{ + // wait for ROS + while(!isControllerAlive()) + { + ros::Duration(0.1).sleep(); + } + // zero everything before test + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.angular.z = 0.0; + publish(cmd_vel); + ros::Duration(2.0).sleep(); + // get initial odom + nav_msgs::Odometry old_odom = getLastOdom(); + // send a big command + cmd_vel.angular.z = 10.0; + publish(cmd_vel); + // wait for a while + ros::Duration(0.5).sleep(); + + nav_msgs::Odometry new_odom = getLastOdom(); + + // check if the robot speed is now 1.0rad.s-1, which is 2.0rad.s-2 * 0.5s + EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), 1.0 + VELOCITY_TOLERANCE); + EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), EPS); + + cmd_vel.angular.z = 0.0; + publish(cmd_vel); +} + +TEST_F(DiffDriveControllerTest, testAngularVelocityLimits) +{ + // wait for ROS + while(!isControllerAlive()) + { + ros::Duration(0.1).sleep(); + } + // zero everything before test + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.angular.z = 0.0; + publish(cmd_vel); + ros::Duration(2.0).sleep(); + // get initial odom + nav_msgs::Odometry old_odom = getLastOdom(); + // send a big command + cmd_vel.angular.z = 10.0; + publish(cmd_vel); + // wait for a while + ros::Duration(5.0).sleep(); + + nav_msgs::Odometry new_odom = getLastOdom(); + + // check if the robot speed is now 2.0rad.s-1, the limit + EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), 2.0 + VELOCITY_TOLERANCE); + EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), EPS); + + cmd_vel.angular.z = 0.0; + publish(cmd_vel); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "diff_drive_limits_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + //ros::Duration(0.5).sleep(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/mecanum_drive_controller/test/diff_drive_multipliers.test b/mecanum_drive_controller/test/diff_drive_multipliers.test new file mode 100644 index 000000000..560330ee2 --- /dev/null +++ b/mecanum_drive_controller/test/diff_drive_multipliers.test @@ -0,0 +1,16 @@ + + + + + + + + + + + + + diff --git a/mecanum_drive_controller/test/diff_drive_odom_tf.test b/mecanum_drive_controller/test/diff_drive_odom_tf.test new file mode 100644 index 000000000..e733666a7 --- /dev/null +++ b/mecanum_drive_controller/test/diff_drive_odom_tf.test @@ -0,0 +1,19 @@ + + + + + + + diffbot_controller: + enable_odom_tf: False + + + + + + + + diff --git a/mecanum_drive_controller/test/diff_drive_odom_tf_test.cpp b/mecanum_drive_controller/test/diff_drive_odom_tf_test.cpp new file mode 100644 index 000000000..41aae1b31 --- /dev/null +++ b/mecanum_drive_controller/test/diff_drive_odom_tf_test.cpp @@ -0,0 +1,59 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2014, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Bence Magyar + +#include "test_common.h" +#include + +// TEST CASES +TEST_F(DiffDriveControllerTest, testNoOdomFrame) +{ + // wait for ROS + while(!isControllerAlive()) + { + ros::Duration(0.1).sleep(); + } + // set up tf listener + tf::TransformListener listener; + ros::Duration(2.0).sleep(); + // check the odom frame doesn't exist + EXPECT_FALSE(listener.frameExists("odom")); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "diff_drive_odom_tf_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/mecanum_drive_controller/test/diff_drive_open_loop.test b/mecanum_drive_controller/test/diff_drive_open_loop.test new file mode 100644 index 000000000..31bf11532 --- /dev/null +++ b/mecanum_drive_controller/test/diff_drive_open_loop.test @@ -0,0 +1,16 @@ + + + + + + + + + + + + + diff --git a/mecanum_drive_controller/test/diff_drive_test.cpp b/mecanum_drive_controller/test/diff_drive_test.cpp new file mode 100644 index 000000000..8f402fb3b --- /dev/null +++ b/mecanum_drive_controller/test/diff_drive_test.cpp @@ -0,0 +1,151 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2013, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Bence Magyar + +#include "test_common.h" +#include + +// TEST CASES +TEST_F(DiffDriveControllerTest, testForward) +{ + // wait for ROS + while(!isControllerAlive()) + { + ros::Duration(0.1).sleep(); + } + // zero everything before test + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.angular.z = 0.0; + publish(cmd_vel); + ros::Duration(0.1).sleep(); + // get initial odom + nav_msgs::Odometry old_odom = getLastOdom(); + // send a velocity command of 0.1 m/s + cmd_vel.linear.x = 0.1; + publish(cmd_vel); + // wait for 10s + ros::Duration(10.0).sleep(); + + nav_msgs::Odometry new_odom = getLastOdom(); + + // check if the robot travelled 1 meter in x, changes in the other fields should be ~~0 + EXPECT_NEAR(fabs(new_odom.pose.pose.position.x - old_odom.pose.pose.position.x), 1.0, POSITION_TOLERANCE); + EXPECT_LT(fabs(new_odom.pose.pose.position.y - old_odom.pose.pose.position.y), EPS); + EXPECT_LT(fabs(new_odom.pose.pose.position.z - old_odom.pose.pose.position.z), EPS); + + // convert to rpy and test that way + double roll_old, pitch_old, yaw_old; + double roll_new, pitch_new, yaw_new; + tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old); + tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new); + EXPECT_LT(fabs(roll_new - roll_old), EPS); + EXPECT_LT(fabs(pitch_new - pitch_old), EPS); + EXPECT_LT(fabs(yaw_new - yaw_old), EPS); + EXPECT_NEAR(fabs(new_odom.twist.twist.linear.x), 0.1, EPS); + EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS); + EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS); + + EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS); + EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS); + EXPECT_LT(fabs(new_odom.twist.twist.angular.z), EPS); +} + +TEST_F(DiffDriveControllerTest, testTurn) +{ + // wait for ROS + while(!isControllerAlive()) + { + ros::Duration(0.1).sleep(); + } + // zero everything before test + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.angular.z = 0.0; + publish(cmd_vel); + ros::Duration(0.1).sleep(); + // get initial odom + nav_msgs::Odometry old_odom = getLastOdom(); + // send a velocity command + cmd_vel.angular.z = M_PI/10.0; + publish(cmd_vel); + // wait for 10s + ros::Duration(10.0).sleep(); + + nav_msgs::Odometry new_odom = getLastOdom(); + + // check if the robot rotated PI around z, changes in the other fields should be ~~0 + EXPECT_LT(fabs(new_odom.pose.pose.position.x - old_odom.pose.pose.position.x), EPS); + EXPECT_LT(fabs(new_odom.pose.pose.position.y - old_odom.pose.pose.position.y), EPS); + EXPECT_LT(fabs(new_odom.pose.pose.position.z - old_odom.pose.pose.position.z), EPS); + + // convert to rpy and test that way + double roll_old, pitch_old, yaw_old; + double roll_new, pitch_new, yaw_new; + tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old); + tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new); + EXPECT_LT(fabs(roll_new - roll_old), EPS); + EXPECT_LT(fabs(pitch_new - pitch_old), EPS); + EXPECT_NEAR(fabs(yaw_new - yaw_old), M_PI, ORIENTATION_TOLERANCE); + + EXPECT_LT(fabs(new_odom.twist.twist.linear.x), EPS); + EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS); + EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS); + + EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS); + EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS); + EXPECT_NEAR(fabs(new_odom.twist.twist.angular.z), M_PI/10.0, EPS); +} + +TEST_F(DiffDriveControllerTest, testOdomFrame) +{ + // wait for ROS + while(!isControllerAlive()) + { + ros::Duration(0.1).sleep(); + } + // set up tf listener + tf::TransformListener listener; + ros::Duration(2.0).sleep(); + // check the odom frame exist + EXPECT_TRUE(listener.frameExists("odom")); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "diff_drive_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + //ros::Duration(0.5).sleep(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/mecanum_drive_controller/test/diff_drive_timeout.test b/mecanum_drive_controller/test/diff_drive_timeout.test new file mode 100644 index 000000000..4563132b3 --- /dev/null +++ b/mecanum_drive_controller/test/diff_drive_timeout.test @@ -0,0 +1,16 @@ + + + + + + + + + + + + + diff --git a/mecanum_drive_controller/test/diff_drive_timeout_test.cpp b/mecanum_drive_controller/test/diff_drive_timeout_test.cpp new file mode 100644 index 000000000..41a12bd79 --- /dev/null +++ b/mecanum_drive_controller/test/diff_drive_timeout_test.cpp @@ -0,0 +1,72 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2013, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Paul Mathieu + +#include "test_common.h" + +// TEST CASES +TEST_F(DiffDriveControllerTest, testTimeout) +{ + // wait for ROS + while(!isControllerAlive()) + { + ros::Duration(0.1).sleep(); + } + // zero everything before test + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.angular.z = 0.0; + publish(cmd_vel); + // give some time to the controller to react to the command + ros::Duration(0.1).sleep(); + // get initial odom + nav_msgs::Odometry old_odom = getLastOdom(); + // send a velocity command of 1 m/s + cmd_vel.linear.x = 1.0; + publish(cmd_vel); + // wait a bit + ros::Duration(3.0).sleep(); + + nav_msgs::Odometry new_odom = getLastOdom(); + + // check if the robot has stopped after 0.5s, thus covering less than 0.5s*1.0m.s-1 + some (big) tolerance + EXPECT_LT(fabs(new_odom.pose.pose.position.x - old_odom.pose.pose.position.x), 0.8); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "diff_drive_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/mecanum_drive_controller/test/diff_drive_wrong.test b/mecanum_drive_controller/test/diff_drive_wrong.test new file mode 100644 index 000000000..bb4164fae --- /dev/null +++ b/mecanum_drive_controller/test/diff_drive_wrong.test @@ -0,0 +1,16 @@ + + + + + + + + + + + + + diff --git a/mecanum_drive_controller/test/diffbot.cpp b/mecanum_drive_controller/test/diffbot.cpp new file mode 100644 index 000000000..60ae82551 --- /dev/null +++ b/mecanum_drive_controller/test/diffbot.cpp @@ -0,0 +1,60 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2013, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +// NOTE: The contents of this file have been taken largely from the ros_control wiki tutorials + +// ROS +#include + +// ros_control +#include + +#include "diffbot.h" + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "diffbot"); + ros::NodeHandle nh; + + Diffbot<> robot; + ROS_WARN_STREAM("period: " << robot.getPeriod().toSec()); + controller_manager::ControllerManager cm(&robot, nh); + + ros::Rate rate(1.0 / robot.getPeriod().toSec()); + ros::AsyncSpinner spinner(1); + spinner.start(); + while(ros::ok()) + { + robot.read(); + cm.update(robot.getTime(), robot.getPeriod()); + robot.write(); + rate.sleep(); + } + spinner.stop(); + + return 0; +} diff --git a/mecanum_drive_controller/test/diffbot.h b/mecanum_drive_controller/test/diffbot.h new file mode 100644 index 000000000..afa082c41 --- /dev/null +++ b/mecanum_drive_controller/test/diffbot.h @@ -0,0 +1,139 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2013, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +// NOTE: The contents of this file have been taken largely from the ros_control wiki tutorials + +// ROS +#include +#include +#include + +// ros_control +#include +#include +#include +#include +#include + +// NaN +#include + +// ostringstream +#include + +template +class Diffbot : public hardware_interface::RobotHW +{ +public: + Diffbot() + : running_(true) + , start_srv_(nh_.advertiseService("start", &Diffbot::start_callback, this)) + , stop_srv_(nh_.advertiseService("stop", &Diffbot::stop_callback, this)) + { + // Intialize raw data + std::fill_n(pos_, NUM_JOINTS, 0.0); + std::fill_n(vel_, NUM_JOINTS, 0.0); + std::fill_n(eff_, NUM_JOINTS, 0.0); + std::fill_n(cmd_, NUM_JOINTS, 0.0); + + // Connect and register the joint state and velocity interface + for (unsigned int i = 0; i < NUM_JOINTS; ++i) + { + std::ostringstream os; + os << "wheel_" << i << "_joint"; + + hardware_interface::JointStateHandle state_handle(os.str(), &pos_[i], &vel_[i], &eff_[i]); + jnt_state_interface_.registerHandle(state_handle); + + hardware_interface::JointHandle vel_handle(jnt_state_interface_.getHandle(os.str()), &cmd_[i]); + jnt_vel_interface_.registerHandle(vel_handle); + } + + registerInterface(&jnt_state_interface_); + registerInterface(&jnt_vel_interface_); + } + + ros::Time getTime() const {return ros::Time::now();} + ros::Duration getPeriod() const {return ros::Duration(0.01);} + + void read() + { + std::ostringstream os; + for (unsigned int i = 0; i < NUM_JOINTS - 1; ++i) + { + os << cmd_[i] << ", "; + } + os << cmd_[NUM_JOINTS - 1]; + + ROS_INFO_STREAM("Commands for joints: " << os.str()); + } + + void write() + { + if (running_) + { + for (unsigned int i = 0; i < NUM_JOINTS; ++i) + { + // Note that pos_[i] will be NaN for one more cycle after we start(), + // but that is consistent with the knowledge we have about the state + // of the robot. + pos_[i] += vel_[i]*getPeriod().toSec(); // update position + vel_[i] = cmd_[i]; // might add smoothing here later + } + } + else + { + std::fill_n(pos_, NUM_JOINTS, std::numeric_limits::quiet_NaN()); + std::fill_n(vel_, NUM_JOINTS, std::numeric_limits::quiet_NaN()); + } + } + + bool start_callback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) + { + running_ = true; + return true; + } + + bool stop_callback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) + { + running_ = false; + return true; + } + +private: + hardware_interface::JointStateInterface jnt_state_interface_; + hardware_interface::VelocityJointInterface jnt_vel_interface_; + double cmd_[NUM_JOINTS]; + double pos_[NUM_JOINTS]; + double vel_[NUM_JOINTS]; + double eff_[NUM_JOINTS]; + bool running_; + + ros::NodeHandle nh_; + ros::ServiceServer start_srv_; + ros::ServiceServer stop_srv_; +}; diff --git a/mecanum_drive_controller/test/diffbot.xacro b/mecanum_drive_controller/test/diffbot.xacro new file mode 100644 index 000000000..f2ace6cc4 --- /dev/null +++ b/mecanum_drive_controller/test/diffbot.xacro @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Green + + + + Gazebo/Purple + + + diff --git a/mecanum_drive_controller/test/diffbot_bad.xacro b/mecanum_drive_controller/test/diffbot_bad.xacro new file mode 100644 index 000000000..ec88c3890 --- /dev/null +++ b/mecanum_drive_controller/test/diffbot_bad.xacro @@ -0,0 +1,142 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + 1 + + + + + + 1 + 1 + + + + + Gazebo/Green + + + + Gazebo/Red + + + + Gazebo/Blue + + + + Gazebo/Purple + + + diff --git a/mecanum_drive_controller/test/diffbot_controllers.yaml b/mecanum_drive_controller/test/diffbot_controllers.yaml new file mode 100644 index 000000000..f89c38c37 --- /dev/null +++ b/mecanum_drive_controller/test/diffbot_controllers.yaml @@ -0,0 +1,8 @@ +diffbot_controller: + type: "diff_drive_controller/DiffDriveController" + left_wheel: 'wheel_0_joint' + right_wheel: 'wheel_1_joint' + publish_rate: 50.0 # defaults to 50 + pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests diff --git a/mecanum_drive_controller/test/diffbot_limits.yaml b/mecanum_drive_controller/test/diffbot_limits.yaml new file mode 100644 index 000000000..fda2cc5b7 --- /dev/null +++ b/mecanum_drive_controller/test/diffbot_limits.yaml @@ -0,0 +1,15 @@ +diffbot_controller: + linear: + x: + has_velocity_limits: true + min_velocity: -0.5 + max_velocity: 1.0 + has_acceleration_limits: true + min_acceleration: -0.5 + max_acceleration: 1.0 + angular: + z: + has_velocity_limits: true + max_velocity: 2.0 + has_acceleration_limits: true + max_acceleration: 2.0 diff --git a/mecanum_drive_controller/test/diffbot_multipliers.yaml b/mecanum_drive_controller/test/diffbot_multipliers.yaml new file mode 100644 index 000000000..d2ef5d5b8 --- /dev/null +++ b/mecanum_drive_controller/test/diffbot_multipliers.yaml @@ -0,0 +1,3 @@ +diffbot_controller: + wheel_separation_multiplier: 2.3 + wheel_radius_multiplier: 1.4 diff --git a/mecanum_drive_controller/test/diffbot_open_loop.yaml b/mecanum_drive_controller/test/diffbot_open_loop.yaml new file mode 100644 index 000000000..19224f3bc --- /dev/null +++ b/mecanum_drive_controller/test/diffbot_open_loop.yaml @@ -0,0 +1,2 @@ +diffbot_controller: + open_loop: true diff --git a/mecanum_drive_controller/test/diffbot_timeout.yaml b/mecanum_drive_controller/test/diffbot_timeout.yaml new file mode 100644 index 000000000..4ecfa1877 --- /dev/null +++ b/mecanum_drive_controller/test/diffbot_timeout.yaml @@ -0,0 +1,2 @@ +diffbot_controller: + cmd_vel_timeout: 0.5 diff --git a/mecanum_drive_controller/test/diffbot_wrong.yaml b/mecanum_drive_controller/test/diffbot_wrong.yaml new file mode 100644 index 000000000..516fd75c3 --- /dev/null +++ b/mecanum_drive_controller/test/diffbot_wrong.yaml @@ -0,0 +1,8 @@ +diffbot_controller: + type: "diff_drive_controller/DiffDriveController" + left_wheel: 'this_joint_does_not_exist' + right_wheel: 'right_wheel_joint' + publish_rate: 50.0 # defaults to 50 + pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests diff --git a/mecanum_drive_controller/test/skid_steer_common.launch b/mecanum_drive_controller/test/skid_steer_common.launch new file mode 100644 index 000000000..fe7e94804 --- /dev/null +++ b/mecanum_drive_controller/test/skid_steer_common.launch @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + diff --git a/mecanum_drive_controller/test/skid_steer_controller.test b/mecanum_drive_controller/test/skid_steer_controller.test new file mode 100644 index 000000000..b89aab12b --- /dev/null +++ b/mecanum_drive_controller/test/skid_steer_controller.test @@ -0,0 +1,13 @@ + + + + + + + + + + diff --git a/mecanum_drive_controller/test/skid_steer_no_wheels.test b/mecanum_drive_controller/test/skid_steer_no_wheels.test new file mode 100644 index 000000000..710096049 --- /dev/null +++ b/mecanum_drive_controller/test/skid_steer_no_wheels.test @@ -0,0 +1,16 @@ + + + + + + + + + + + + + diff --git a/mecanum_drive_controller/test/skidsteerbot.cpp b/mecanum_drive_controller/test/skidsteerbot.cpp new file mode 100644 index 000000000..37ca8c3f4 --- /dev/null +++ b/mecanum_drive_controller/test/skidsteerbot.cpp @@ -0,0 +1,60 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2013, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +// NOTE: The contents of this file have been taken largely from the ros_control wiki tutorials + +// ROS +#include + +// ros_control +#include + +#include "diffbot.h" + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "skidsteerbot"); + ros::NodeHandle nh; + + Diffbot<6> robot; + ROS_WARN_STREAM("period: " << robot.getPeriod().toSec()); + controller_manager::ControllerManager cm(&robot, nh); + + ros::Rate rate(1.0 / robot.getPeriod().toSec()); + ros::AsyncSpinner spinner(1); + spinner.start(); + while(ros::ok()) + { + robot.read(); + cm.update(robot.getTime(), robot.getPeriod()); + robot.write(); + rate.sleep(); + } + spinner.stop(); + + return 0; +} diff --git a/mecanum_drive_controller/test/skidsteerbot.xacro b/mecanum_drive_controller/test/skidsteerbot.xacro new file mode 100644 index 000000000..657602655 --- /dev/null +++ b/mecanum_drive_controller/test/skidsteerbot.xacro @@ -0,0 +1,89 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Green + + + + Gazebo/Purple + + + diff --git a/mecanum_drive_controller/test/skidsteerbot_controllers.yaml b/mecanum_drive_controller/test/skidsteerbot_controllers.yaml new file mode 100644 index 000000000..ab4942480 --- /dev/null +++ b/mecanum_drive_controller/test/skidsteerbot_controllers.yaml @@ -0,0 +1,8 @@ +skidsteerbot_controller: + type: "diff_drive_controller/DiffDriveController" + left_wheel: ['wheel_0_joint', 'wheel_1_joint', 'wheel_2_joint'] + right_wheel: ['wheel_3_joint', 'wheel_4_joint', 'wheel_5_joint'] + publish_rate: 50.0 # defaults to 50 + pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests diff --git a/mecanum_drive_controller/test/skidsteerbot_no_wheels.yaml b/mecanum_drive_controller/test/skidsteerbot_no_wheels.yaml new file mode 100644 index 000000000..37f0d99dd --- /dev/null +++ b/mecanum_drive_controller/test/skidsteerbot_no_wheels.yaml @@ -0,0 +1,8 @@ +skidsteerbot_controller: + type: "diff_drive_controller/DiffDriveController" + left_wheel: [] + right_wheel: ['wheel_3_joint', 'wheel_4_joint', 'wheel_5_joint'] + publish_rate: 50.0 # defaults to 50 + pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests diff --git a/mecanum_drive_controller/test/test_common.h b/mecanum_drive_controller/test/test_common.h new file mode 100644 index 000000000..9738d9c88 --- /dev/null +++ b/mecanum_drive_controller/test/test_common.h @@ -0,0 +1,85 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2013, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Bence Magyar + +#include + +#include + +#include + +#include +#include +#include + +// Floating-point value comparison threshold +const double EPS = 0.01; +const double POSITION_TOLERANCE = 0.01; // 1 cm-s precision +const double VELOCITY_TOLERANCE = 0.02; // 2 cm-s-1 precision +const double ORIENTATION_TOLERANCE = 0.03; // 0.57 degree precision + +class DiffDriveControllerTest : public ::testing::Test +{ +public: + + DiffDriveControllerTest() + : cmd_pub(nh.advertise("cmd_vel", 100)), + odom_sub(nh.subscribe("odom", 100, &DiffDriveControllerTest::odomCallback, this)) + { + } + + ~DiffDriveControllerTest() + { + odom_sub.shutdown(); + } + + nav_msgs::Odometry getLastOdom(){ return last_odom; } + void publish(geometry_msgs::Twist cmd_vel){ cmd_pub.publish(cmd_vel); } + bool isControllerAlive(){ return (odom_sub.getNumPublishers() > 0) && (cmd_pub.getNumSubscribers() > 0); } + +private: + ros::NodeHandle nh; + ros::Publisher cmd_pub; + ros::Subscriber odom_sub; + nav_msgs::Odometry last_odom; + + void odomCallback(const nav_msgs::Odometry& odom) + { + ROS_INFO_STREAM("Callback reveived: pos.x: " << odom.pose.pose.position.x + << ", orient.z: " << odom.pose.pose.orientation.z + << ", lin_est: " << odom.twist.twist.linear.x + << ", ang_est: " << odom.twist.twist.angular.z); + last_odom = odom; + } +}; + +inline tf::Quaternion tfQuatFromGeomQuat(const geometry_msgs::Quaternion& quat) +{ + return tf::Quaternion(quat.x, quat.y, quat.z, quat.w); +} + diff --git a/mecanum_drive_controller/test/view_diffbot.launch b/mecanum_drive_controller/test/view_diffbot.launch new file mode 100644 index 000000000..cdc362e93 --- /dev/null +++ b/mecanum_drive_controller/test/view_diffbot.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/mecanum_drive_controller/test/view_skidsteerbot.launch b/mecanum_drive_controller/test/view_skidsteerbot.launch new file mode 100644 index 000000000..66d8d5c2b --- /dev/null +++ b/mecanum_drive_controller/test/view_skidsteerbot.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/mecanum_drive_controller/test/wheel.xacro b/mecanum_drive_controller/test/wheel.xacro new file mode 100644 index 000000000..6fa1bd60c --- /dev/null +++ b/mecanum_drive_controller/test/wheel.xacro @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + 1 + + + + Gazebo/Red + + +