Mecanum drive

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.

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})

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

mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.h

@@ -0,0 +1,148 @@
+#pragma once
+
+#include <memory>
+
+#include <controller_interface/controller.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <pluginlib/class_list_macros.h>
+
+#include <nav_msgs/Odometry.h>
+#include <tf/tfMessage.h>
+
+#include <realtime_tools/realtime_buffer.h>
+#include <realtime_tools/realtime_publisher.h>
+
+#include <mecanum_drive_controller/odometry.h>
+
+namespace mecanum_drive_controller
+{
+// Check file README.md for restrictions and notes.
+class MecanumDriveController : public controller_interface::Controller<hardware_interface::VelocityJointInterface>
+{
+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> command_rt_buffer_;
+  Command command_;
+  ros::Subscriber command_sub_;
+
+  /// Odometry related:
+  Odometry odometry_;
+
+  std::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::Odometry> > odom_pub_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > 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

mecanum_drive_controller/include/mecanum_drive_controller/odometry.h

@@ -0,0 +1,114 @@
+#ifndef ODOMETRY_H_
+#define ODOMETRY_H_
+
+#include <ros/time.h>
+#include <boost/accumulators/accumulators.hpp>
+#include <boost/accumulators/statistics/stats.hpp>
+#include <boost/accumulators/statistics/rolling_mean.hpp>
+#include <boost/function.hpp>
+
+#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<void(double, double, double)> IntegrationFunction;
+
+  /// \brief Constructor
+  /// Timestamp will get the current time value
+  /// Value will be set to zero
+  /// \param velocity_rolling_window_size Rolling window size used to compute the velocity mean
+  Odometry(size_t velocity_rolling_window_size = 10);
+
+  /// \brief Initialize the odometry
+  /// \param time Current time
+  void init(const ros::Time& time, 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<double, bacc::stats<bacc::tag::rolling_mean> > 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_ */

mecanum_drive_controller/mecanum_drive_controller_plugins.xml

@@ -0,0 +1,9 @@
+<library path="lib/libmecanum_drive_controller">
+
+  <class name="mecanum_drive_controller/MecanumDriveController" type="mecanum_drive_controller::MecanumDriveController" base_class_type="controller_interface::ControllerBase">
+    <description>
+      The MecanumDriveController tracks velocity commands. It 4 joints should have a VelocityJointInterface hardware interface.
+    </description>
+  </class>
+
+</library>

mecanum_drive_controller/package.xml

@@ -0,0 +1,28 @@
+<package>
+  <name>mecanum_drive_controller</name>
+  <version>0.0.1</version>
+  <description>Controller for a mecanum drive mobile base.</description>
+  <maintainer email="[email protected]">Antoine Rennuit</maintainer>
+
+  <license>MIT</license>
+
+  <author email="[email protected]">Antoine Rennuit</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>controller_interface</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>realtime_tools</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>urdf</build_depend>
+
+  <run_depend>controller_interface</run_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>realtime_tools</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>urdf</run_depend>
+
+  <export>
+    <controller_interface plugin="${prefix}/mecanum_drive_controller_plugins.xml"/>
+  </export>
+</package>