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>