diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100755
index 0000000000000000000000000000000000000000..8bafa33466036ee9c0c47de39f63a96677b5c0de
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,44 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mecanum_drive)
+
+find_package(catkin REQUIRED COMPONENTS
+ rospy
+ std_msgs
+ nav_msgs
+ geometry_msgs
+ tf
+ tf2
+ actionlib
+ actionlib_msgs
+)
+
+add_action_files(
+ DIRECTORY action
+ FILES GoToPose.action
+)
+
+if (CATKIN_ENABLE_TESTING)
+ catkin_add_nosetests(tests)
+endif()
+
+if (CATKIN_ENABLE_TESTING)
+ find_package(rostest REQUIRED)
+# add_rostest(tests/your_first_rostest.test)
+# add_rostest(tests/your_second_rostest.test)
+endif()
+
+catkin_install_python(
+ PROGRAMS
+ nodes/mecanum_drive_odometry
+ nodes/mecanum_drive_controller
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+catkin_python_setup()
+
+generate_messages (
+ DEPENDENCIES actionlib_msgs std_msgs geometry_msgs
+)
+
+catkin_package(
+ CATKIN_DEPENDS actionlib_msgs
+)
diff --git a/LICENSE b/LICENSE
new file mode 100755
index 0000000000000000000000000000000000000000..862674582582c9e4963d6537f3e573321111056a
--- /dev/null
+++ b/LICENSE
@@ -0,0 +1,27 @@
+Copyright (c) 2016, Mark Rose
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+* Neither the name of diff_drive 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 HOLDER 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.
diff --git a/README.asciidoc b/README.asciidoc
new file mode 100755
index 0000000000000000000000000000000000000000..54127dc8c084b6f60918efe3f4c5434b1605fd38
--- /dev/null
+++ b/README.asciidoc
@@ -0,0 +1,234 @@
+= diff_drive -- Differential-Drive Controller
+:imagesdir: ./images
+
+This package implements ROS nodes to control and monitor a differential-drive robot.
+
+The package
+is intended as a lighter-weight solution than the ROS controller framework, albeit with lower
+performance since it is written in Python. If you need tight, real-time control, you may want
+to look at link:http://wiki.ros.org/ros_controllers[ros_controllers],
+a C++ package which includes a differential-drive controller that is integrated with
+link:http://wiki.ros.org/ros_control[ros_control] and
+link:http://wiki.ros.org/controller_manager[controller_manager]. Those controllers are designed
+to integrate with hardware in the same process, rather than using topics. Instead, this package
+expects to publish the desired motor speeds using standard ROS messages.
+
+== Supplied Nodes
+
+* `diff_drive_controller` -- Converts from twist to wheel velocities for motors.
+* `diff_drive_odometry` -- Publishes odometry from wheel encoder data.
+* `diff_drive_go_to_goal` -- Moves the robot to a goal position.
+* `diff_drive_mock_robot` -- Implements a mock differential drive robot, for testing.
+
+The nodes in this package are designed with these considerations:
+
+* The node and hardware implementing differential drive should deal only in encoder ticks.
+* Conversions to and from physical coordinates should happen within the nodes in this package.
+* This package should integrate cleanly with the navigation stack, perhaps with remappings.
+* Nodes should use standard topic and parameter names used by the navigation stack, but should allow remapping.
+
+== Demo
+
+To see all the nodes in this package in action, you can launch a demo from ROS. There are no
+dependencies other than the standard ROS packages.
+
+ roslaunch diff_drive demo.launch
+
+This launches _rviz_ as part of the demo, and shows the robot position as a small coordinate system on
+a 0.25m grid. In rviz you can move the robot by clicking the _2D Nav Goal_ button in the tools panel at the top.
+Then click and drag within the grid to set the robot goal position and heading. The mock robot will move to
+that new pose, which you can see by the movement of the robot axes.
+
+In the demo, both forward and backward movement is allowed, so if the goal position is behind the robot,
+it will move backward. You can force the robot to move foward only by setting the parameter `~forwardMovementOnly`
+to `true`.
+
+== ROS API
+
+=== 1. diff_drive_controller
+
+Listens for desired linear and angular velocity, and publishes corresponding wheel velocities, in encoder ticks per second, required to achieve those velocities.
+
+==== Published Topics
+
+`~lwheel_desired_rate` (std_msgs/Int32)::
+Desired left wheel rotation rate, in encoder ticks per second.
+
+`~rwheel_desired_rate` (std_msgs/Int32)::
+Desired right wheel rotation rate, in encoder ticks per second.
+
+==== Subscribed Topics
+
+`~cmd_vel` (geometry_msgs/Twist)::
+Desired linear and angular velocity.
+
+==== Parameters
+
+`~ticks_per_meter` (double)::
+Number of encoder ticks per meter of travel.
+
+`~wheel_separation` (double)::
+Distance between the two wheels (meters).
+
+`~rate` (int, default: 50)::
+The rate that the output velocity target messages will be published (Hz).
+
+`~timeout_ticks` (int, default: 2)::
+The number of velocity target messages that will be published after the last twist message is received.
+
+=== 2. diff_drive_odometry
+
+Listens for wheel movement and rates and publishes the transform between the odom frame and the robot frame.
+
+==== Published Topics
+
+`~odom` -- (nav_msgs/Odometry)::
+The robot odometry -- the current robot pose.
+
+`~tf`::
+The transform between the odometry frame and the robot frame.
+
+==== Subscribed Topics
+
+`~lwheel_ticks` (std_msgs/Int32)::
+Cumulative encoder ticks of the left wheel.
+
+`~rwheel_ticks` (std_msgs/Int32)::
+Cumulative encoder ticks of the right wheel.
+
+`~lwheel_rate` (std_msgs/Float32)::
+Left wheel rotation rate, in encoder ticks per second.
+
+`~rwheel_rate` (std_msgs/Float32)::
+Right wheel rotation rate, in encoder ticks per second.
+
+==== Parameters
+
+`~ticks_per_meter` (double)::
+Number of encoder ticks per meter of travel.
+
+`~wheel_separation` (double)::
+Distance between the two wheels (m).
+
+`~rate` (double, default 10.0)::
+The rate at which the `tf` and `odom` topics are published (Hz).
+
+`~timeout` (double, default 0.2)::
+The amount of time to continue publishing desired wheel rates after receiving a twist message (seconds).
+If set to zero, wheel velocities will be sent only when a new twist message is received.
+
+`~base_frame_id` (string, default: "base_link")::
+The name of the base frame of the robot.
+
+`~odom_frame_id` (string, default: "odom")::
+The name of the odometry reference frame.
+
+`~encoder_min` (int, default: -32768)::
+
+`~encoder_max` (int, default: 32768)::
+The min and max value the encoder should output. Used to calculate odometry when the values wrap around.
+
+`~wheel_low_wrap` (int, default: 0.3 * (encoder_max - encoder_min + 1) + encoder_min)::
+
+`~wheel_high_wrap` (int, default: 0.7 * (encoder_max - encoder_min + 1) + encoder_min)::
+If a reading is greater than wheel_high_wrap and the next reading is less than wheel_low_wrap, then the reading has wrapped around in the positive direction, and the odometry will be calculated appropriately. The same concept applies for the negative direction.
+
+=== 3. diff_drive_go_to_goal
+
+Listens for new goal poses and computes velocities needed to achieve the goal.
+
+==== Published Topics
+
+`~distance_to_goal` (std_msgs/Float32)::
+Distance to the goal position (meters).
+
+`~cmd_vel` (geometry_msgs/Twist)::
+Desired linear and angular velocity to move toward the goal pose.
+
+==== Subscribed Topics
+
+`~goal` (geometry_msgs/Pose)::
+Desired goal pose.
+
+==== Parameters
+
+`~rate` (float, default: 10)::
+Rate at which to publish desired velocities (Hz).
+
+`~goal_linear_tolerance` (float, default: 0.1)::
+The distance from the goal at which the robot is assumed to have accomplished the goal position (meters).
+
+`~goal_angular_tolerance` (float, default: 0.087)::
+The difference between robot angle and goal pose angle at which the robot is assumed to have
+accomplished the goal attitude (radians). Default value is approximately 5 degrees.
+
+`~max_linear_velocity` (float, default: 0.2)::
+The maximum linear velocity toward the goal (meters/second).
+
+`~max_angular_velocity` (float, default: 1.5)::
+The maximum angular velocity (radians/second).
+
+`~max_linear_acceleration` (float, default: 4.0)::
+The maximum linear acceleration (meters/second^2).
+
+`~forwardMovementOnly` (boolean, default: true)::
+If true, only forward movement is allowed to achieve the goal position.
+If false, the robot will move backward to the goal if that is the most
+direct path.
+
+`~Kp` (float, default: 3.0)::
+Linear distance proportionality constant. Higher values make the robot accelerate more quickly toward the goal and decelerate less quickly.
+
+`~Ka` (float: default: 8.0)::
+Proportionality constant for angle to goal position. Higher values make the robot turn more quickly toward the goal.
+
+`~Kb` (float: default: -1.5)::
+Proportionality constant for angle to goal pose direction. Higher values make the robot turn more quickly toward the goal pose direction. This value should be negative, per _Autonomous Mobile Robots_.
+
+The control law for determining the linear and angular velocity to move toward the goal works as follows. Let _d_ be the distance to the goal. Let _a_ be the angle between the robot heading and the goal position, where left is positive. Let _b_ be the angle between the goal direction and the final pose angle, where left is positive. Then the robot linear and angular velocities are calculated like this:
+
+ v = Kp * d
+ w = Ka*a + Kb*b
+
+See _Autonomous Mobile Robots, Second Edition_ by Siegwart et. al., section 3.6.2.4. In this code, when the robot
+is near enough to the goal, _v_ is set to zero, and _w_ is simply _Kb*b_.
+
+To ensure convergence toward the goal, _K~p~_ and _K~a~_ must be positive, _K~b~_ must be negative, and _K~a~_
+must be greater than _K~p~_. To ensure robust convergence, so that the robot never changes direction,
+_K~a~_ - 5/3*_K~b~_ - 2/pi*_K~p~_ must be greater than zero.
+
+=== 4. diff_drive_mock_robot
+
+Implements a simulation of perfect differential drive robot hardware. It immediately follows any speed
+commands received with infinite acceleration, and publishes the wheel encoder values and encoder
+rates.
+
+==== Published Topics
+
+`~lwheel_ticks` (std_msgs/Int32)::
+Cumulative encoder ticks of the left wheel.
+
+`~rwheel_ticks` (std_msgs/Int32)::
+Cumulative encoder ticks of the right wheel.
+
+`~lwheel_rate` (std_msgs/Float32)::
+Left wheel rotation rate, in encoder ticks per second.
+
+`~rwheel_rate` (std_msgs/Float32)::
+Right wheel rotation rate, in encoder ticks per second.
+
+==== Subscribed Topics
+
+`~lwheel_desired_rate` (std_msgs/Int32)::
+Desired left wheel rotation rate, in encoder ticks per second.
+
+`~rwheel_desired_rate` (std_msgs/Int32)::
+Desired right wheel rotation rate, in encoder ticks per second.
+
+==== Parameters
+
+`~cmd_timeout` (float, default: 0.2)::
+The amount of time after the last wheel rate message when the robot should stop automatically (seconds).
+
+`~rate` (float, default 10.0)::
+The rate at which the simulated wheel encoder values and rates should be published (Hz).
diff --git a/action/GoToPose.action b/action/GoToPose.action
new file mode 100755
index 0000000000000000000000000000000000000000..1cf9b79dcffbc9cd9585562fd9d78351998b542b
--- /dev/null
+++ b/action/GoToPose.action
@@ -0,0 +1,9 @@
+# goal definition
+geometry_msgs/PoseStamped pose
+---
+# result definition
+bool success
+---
+# feedback
+bool processing
+
diff --git a/launch/demo.launch b/launch/demo.launch
new file mode 100755
index 0000000000000000000000000000000000000000..60711d53224ea1a1b42680d332a897122523aef8
--- /dev/null
+++ b/launch/demo.launch
@@ -0,0 +1,68 @@
+<launch>
+
+ <arg name="ticks_per_meter" value="10000" />
+ <arg name="wheel_separation" value="0.2" />
+
+ <arg name="urdf_file" default="$(find diff_drive)/urdf/diff_drive_description.xacro" />
+ <param name="robot_description" command="$(find xacro)/xacro --inorder $(arg urdf_file)" />
+
+ <!-- Publish the robot state -->
+ <node name="robot_state_publisher" pkg="robot_state_publisher"
+ type="robot_state_publisher">
+ <param name="publish_frequency" value="10.0"/>
+ </node>
+
+ <!-- Provide simulated control of the robot joint angles -->
+ <node name="joint_state_publisher" pkg="joint_state_publisher"
+ type="joint_state_publisher">
+ <param name="use_gui" value="False" />
+ <param name="rate" value="10.0"/>
+ </node>
+
+ <node name="controller" pkg="diff_drive" type="diff_drive_controller"
+ output="screen">
+ <rosparam subst_value="true">
+ ticks_per_meter: $(arg ticks_per_meter)
+ wheel_separation: $(arg wheel_separation)
+ max_motor_speed: 3000
+ timeout: 1.0
+ </rosparam>
+ <remap from="cmd_vel" to="/robot/cmd_vel" />
+ </node>
+
+ <node name="odom_publisher" pkg="diff_drive" type="diff_drive_odometry"
+ output="screen">
+ <rosparam subst_value="true">
+ ticks_per_meter: $(arg ticks_per_meter)
+ wheel_separation: $(arg wheel_separation)
+ </rosparam>
+ </node>
+
+ <node name="robot" pkg="diff_drive" type="diff_drive_mock_robot"
+ output="screen">
+ <remap from="~lwheel_desired_rate" to="lwheel_desired_rate" />
+ <remap from="~rwheel_desired_rate" to="rwheel_desired_rate" />
+ <remap from="~lwheel_ticks" to="lwheel_ticks" />
+ <remap from="~rwheel_ticks" to="rwheel_ticks" />
+ </node>
+
+ <node name="diff_drive_go_to_goal" pkg="diff_drive"
+ type="diff_drive_go_to_goal" output="screen">
+ <param name="~rate" value="20" />
+ <param name="~kP" value="0.5" />
+ <param name="~kA" value="1.0" />
+ <param name="~kB" value="-0.8" />
+ <param name="~max_linear_speed" value="0.2" />
+ <param name="~min_linear_speed" value="0.05" />
+ <param name="~max_angular_speed" value="0.7" />
+ <param name="~min_angular_speed" value="0.1" />
+ <param name="~linear_tolerance" value="0.01" />
+ <param name="~angular_tolerance" value="0.04" />
+ <param name="~forwardMovementOnly" value="false" />
+ <remap from="cmd_vel" to="/robot/cmd_vel" />
+ </node>
+
+ <node name="rviz" pkg="rviz" type="rviz"
+ args="-d $(find diff_drive)/config/view.rviz" />
+
+</launch>
diff --git a/launch/revvy-config.launch b/launch/revvy-config.launch
new file mode 100755
index 0000000000000000000000000000000000000000..39ab4befdad8ccfdd5b6228b3817567be368e9eb
--- /dev/null
+++ b/launch/revvy-config.launch
@@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+<launch>
+
+ <arg name="ticks_per_meter" value="1800" />
+ <arg name="wheel_separation" value="0.1" />
+
+ <arg name="urdf_file" default="$(find diff_drive)/urdf/diff_drive_description.xacro" />
+ <param name="robot_description" command="$(find xacro)/xacro --inorder $(arg urdf_file)" />
+
+ <!-- Publish the robot state -->
+ <node name="robot_state_publisher" pkg="robot_state_publisher"
+ type="robot_state_publisher">
+ <param name="publish_frequency" value="10.0"/>
+ </node>
+
+ <!-- Provide simulated control of the robot joint angles -->
+ <node name="joint_state_publisher" pkg="joint_state_publisher"
+ type="joint_state_publisher">
+ <param name="use_gui" value="False" />
+ <param name="rate" value="10.0"/>
+ </node>
+
+ <node name="controller" pkg="diff_drive" type="diff_drive_controller"
+ output="screen">
+ <rosparam subst_value="true">
+ ticks_per_meter: $(arg ticks_per_meter)
+ wheel_separation: $(arg wheel_separation)
+ max_motor_speed: 3000
+ timeout: 1.0
+ </rosparam>
+ </node>
+
+ <node name="odom_publisher" pkg="diff_drive" type="diff_drive_odometry"
+ output="screen">
+ <rosparam subst_value="true">
+ ticks_per_meter: $(arg ticks_per_meter)
+ wheel_separation: $(arg wheel_separation)
+ </rosparam>
+ </node>
+
+
+</launch>
diff --git a/nodes/mecanum_drive_controller b/nodes/mecanum_drive_controller
new file mode 100755
index 0000000000000000000000000000000000000000..d788a3a99bbab788a3072276e58201e9822ee826
--- /dev/null
+++ b/nodes/mecanum_drive_controller
@@ -0,0 +1,68 @@
+#! /usr/bin/env python
+from __future__ import division
+
+import rospy
+from geometry_msgs.msg import Twist
+from std_msgs.msg import Int16, Int16MultiArray
+
+from mecanum_drive import controller
+
+class ControllerNode:
+
+ def __init__(self):
+ self.controller = controller.Controller()
+ self.linearXVelocity = 0.0
+ self.linearYVelocity = 0.0
+ self.angularVelocity = 0.0
+ self.wheels_to_send = Int16MultiArray()
+
+ def main(self):
+ self.wheelPub = rospy.Publisher('wheels_desired_rate',
+ Int16MultiArray, queue_size=1)
+
+ rospy.init_node('mecanum_drive_controller')
+ self.nodeName = rospy.get_name()
+ rospy.loginfo("{0} started".format(self.nodeName))
+
+ rospy.Subscriber("cmd_vel", Twist, self.twistCallback)
+
+ self.ticksPerMeter = float(rospy.get_param('~ticks_per_meter'))
+ self.wheelSeparation = float(rospy.get_param('~wheel_separation'))
+ self.maxMotorSpeed = int(rospy.get_param('~max_motor_speed'))
+ self.rate = float(rospy.get_param('~rate', 10.0))
+ self.timeout = float(rospy.get_param('~timeout', 0.2))
+
+ self.controller.setWheelSeparation(self.wheelSeparation)
+ self.controller.setTicksPerMeter(self.ticksPerMeter)
+ self.controller.setMaxMotorSpeed(self.maxMotorSpeed)
+
+ rate = rospy.Rate(self.rate)
+ self.lastTwistTime = rospy.get_time()
+ while not rospy.is_shutdown():
+ self.publish()
+ rate.sleep()
+
+ def publish(self):
+ if rospy.get_time() - self.lastTwistTime < self.timeout:
+ speeds = self.controller.getSpeeds(self.linearXVelocity,self.linearYVelocity,
+ self.angularVelocity)
+
+ self.wheels_to_send.data = [int(speeds.frontLeft), int(speeds.frontRight), int(speeds.rearLeft), int(speeds.rearRight)]
+ self.wheelPub.publish(self.wheels_to_send)
+ else:
+ self.wheels_to_send.data = [0, 0, 0, 0]
+ self.wheelPub.publish(self.wheels_to_send)
+
+ def twistCallback(self, twist):
+ self.linearXVelocity = twist.linear.x
+ self.linearYVelocity = twist.linear.y
+ self.angularVelocity = twist.angular.z
+ # print("%d,%d" % (self.linearVelocity, self.angularVelocity))
+ self.lastTwistTime = rospy.get_time()
+
+if __name__ == '__main__':
+ try:
+ node = ControllerNode()
+ node.main()
+ except rospy.ROSInterruptException:
+ pass
diff --git a/nodes/mecanum_drive_odometry b/nodes/mecanum_drive_odometry
new file mode 100755
index 0000000000000000000000000000000000000000..74c05acdf94e21a525144426e5daf03c296ab5b0
--- /dev/null
+++ b/nodes/mecanum_drive_odometry
@@ -0,0 +1,123 @@
+#! /usr/bin/env python
+from __future__ import division
+
+import rospy
+from geometry_msgs.msg import Quaternion
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from std_msgs.msg import Int32
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from tf.broadcaster import TransformBroadcaster
+from tf.transformations import quaternion_from_euler, euler_from_quaternion
+from math import sin, cos
+from mecanum_drive.pose import Pose
+from mecanum_drive import odometry
+
+ODOM_POSE_COVARIANCE = [1e-3, 0, 0, 0, 0, 0,
+ 0, 1e-3, 0, 0, 0, 0,
+ 0, 0, 1e6, 0, 0, 0,
+ 0, 0, 0, 1e6, 0, 0,
+ 0, 0, 0, 0, 1e6, 0,
+ 0, 0, 0, 0, 0, 1e3]
+
+ODOM_TWIST_COVARIANCE = [1e-3, 0, 0, 0, 0, 0,
+ 0, 1e-3, 0, 0, 0, 0,
+ 0, 0, 1e6, 0, 0, 0,
+ 0, 0, 0, 1e6, 0, 0,
+ 0, 0, 0, 0, 1e6, 0,
+ 0, 0, 0, 0, 0, 1e3]
+
+class OdometryNode:
+
+ def __init__(self):
+ self.odometry = odometry.Odometry()
+
+ def main(self):
+ self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
+ self.tfPub = TransformBroadcaster()
+
+ rospy.init_node('mecanum_drive_odometry')
+ self.nodeName = rospy.get_name()
+ rospy.loginfo("{0} started".format(self.nodeName))
+
+ rospy.Subscriber("lwheel_ticks", Int32, self.leftCallback)
+ rospy.Subscriber("rwheel_ticks", Int32, self.rightCallback)
+ rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
+ self.on_initial_pose)
+
+ self.ticksPerMeter = int(rospy.get_param('~ticks_per_meter'))
+ self.wheelSeparation = float(rospy.get_param('~wheel_separation'))
+ self.rate = float(rospy.get_param('~rate', 20.0))
+ self.baseFrameID = rospy.get_param('~base_frame_id', 'base_link')
+ self.odomFrameID = rospy.get_param('~odom_frame_id', 'odom')
+ self.encoderMin = int(rospy.get_param('~encoder_min', -32768))
+ self.encoderMax = int(rospy.get_param('~encoder_max', 32767))
+
+ self.odometry.setWheelSeparation(self.wheelSeparation)
+ self.odometry.setTicksPerMeter(self.ticksPerMeter)
+ self.odometry.setEncoderRange(self.encoderMin, self.encoderMax)
+ self.odometry.setTime(rospy.get_time())
+
+ rate = rospy.Rate(self.rate)
+ while not rospy.is_shutdown():
+ self.publish()
+ rate.sleep()
+
+ def publish(self):
+ self.odometry.updatePose(rospy.get_time())
+ now = rospy.get_rostime()
+ pose = self.odometry.getPose()
+
+ q = quaternion_from_euler(0, 0, pose.theta)
+ self.tfPub.sendTransform(
+ (pose.x, pose.y, 0),
+ (q[0], q[1], q[2], q[3]),
+ now,
+ self.baseFrameID,
+ self.odomFrameID
+ )
+
+ odom = Odometry()
+ odom.header.stamp = now
+ odom.header.frame_id = self.odomFrameID
+ odom.child_frame_id = self.baseFrameID
+ odom.pose.pose.position.x = pose.x
+ odom.pose.pose.position.y = pose.y
+ odom.pose.pose.orientation.x = q[0]
+ odom.pose.pose.orientation.y = q[1]
+ odom.pose.pose.orientation.z = q[2]
+ odom.pose.pose.orientation.w = q[3]
+ odom.twist.twist.linear.x = pose.xVel
+ odom.twist.twist.angular.z = pose.thetaVel
+ odom.pose.covariance = ODOM_POSE_COVARIANCE
+ odom.twist.covariance = ODOM_TWIST_COVARIANCE
+ self.odomPub.publish(odom)
+
+ def on_initial_pose(self, msg):
+ q = [msg.pose.pose.orientation.x,
+ msg.pose.pose.orientation.x,
+ msg.pose.pose.orientation.x,
+ msg.pose.pose.orientation.w]
+ roll, pitch, yaw = euler_from_quaternion(q)
+
+ pose = Pose()
+ pose.x = msg.pose.pose.position.x
+ pose.y = msg.pose.pose.position.y
+ pose.theta = yaw
+
+ rospy.loginfo('Setting initial pose to %s', pose)
+ self.odometry.setPose(pose)
+
+ def leftCallback(self, msg):
+ self.odometry.updateLeftWheel(msg.data)
+
+ def rightCallback(self, msg):
+ self.odometry.updateRightWheel(msg.data)
+
+
+if __name__ == '__main__':
+ try:
+ node = OdometryNode()
+ node.main()
+ except rospy.ROSInterruptException:
+ pass
diff --git a/package.xml b/package.xml
new file mode 100755
index 0000000000000000000000000000000000000000..eb225cf173f90849fb51c3197aeb674268445ae2
--- /dev/null
+++ b/package.xml
@@ -0,0 +1,22 @@
+<?xml version="1.0" encoding="utf-8"?>
+<package format="2">
+ <name>mecanum_drive</name>
+ <version>1.0.0</version>
+ <description>
+ This package implements a control mechanism for a mecanum drive robot.
+ </description>
+ <author>David Dudas</author>
+ <maintainer email="david.dudas@outlook.com">David Dudas</maintainer>
+ <url type="website">https://github.com/dudasdavid/mecanum_drive</url>
+ <url type="repository">https://github.com/dudasdavid/mecanum_drive.git</url>
+ <url type="bugtracker">https://github.com/dudasdavid/mecanum_drive/issues</url>
+ <license>BSD</license>
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <depend>std_msgs</depend>
+ <depend>geometry_msgs</depend>
+ <depend>actionlib_msgs</depend>
+ <build_depend>rostest</build_depend>
+ <depend>message_generation</depend>
+
+</package>
diff --git a/setup.py b/setup.py
new file mode 100755
index 0000000000000000000000000000000000000000..dec9110728116c31ae11b3a5825cea46fbb1c708
--- /dev/null
+++ b/setup.py
@@ -0,0 +1,12 @@
+## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+# fetch values from package.xml
+setup_args = generate_distutils_setup(
+ packages=['mecanum_drive'],
+ package_dir={'': 'src'})
+
+setup(**setup_args)
+
diff --git a/src/mecanum_drive/__init__.py b/src/mecanum_drive/__init__.py
new file mode 100755
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/mecanum_drive/controller.py b/src/mecanum_drive/controller.py
new file mode 100755
index 0000000000000000000000000000000000000000..c7f8be49b3d6b5a9a468b679e6373aee1fa8ef46
--- /dev/null
+++ b/src/mecanum_drive/controller.py
@@ -0,0 +1,62 @@
+from __future__ import division
+
+class MotorCommand:
+ """Holds motor control commands for a differential-drive robot.
+ """
+
+ def __init__(self):
+ self.frontLeft = 0
+ self.frontRight = 0
+ self.rearLeft = 0
+ self.rearRight = 0
+
+
+class Controller:
+ """Determines motor speeds to accomplish a desired motion.
+ """
+
+ def __init__(self):
+ # Set the max motor speed to a very large value so that it
+ # is, essentially, unbound.
+ self.maxMotorSpeed = 10000 # ticks/s
+
+ def getSpeeds(self, linearXSpeed, linearYSpeed, angularSpeed):
+
+
+ # print(linearXSpeed,linearYSpeed,angularSpeed)
+
+ WHEEL_SEPARATION_WIDTH = 0.1
+ WHEEL_SEPARATION_LENGTH = 0.13
+
+ speeds = MotorCommand()
+
+ speeds.frontLeft = self.ticksPerMeter * (linearXSpeed - linearYSpeed - (WHEEL_SEPARATION_WIDTH + WHEEL_SEPARATION_LENGTH)*angularSpeed)
+ speeds.frontRight = self.ticksPerMeter * (linearXSpeed + linearYSpeed + (WHEEL_SEPARATION_WIDTH + WHEEL_SEPARATION_LENGTH)*angularSpeed)
+ speeds.rearLeft = self.ticksPerMeter * (linearXSpeed + linearYSpeed - (WHEEL_SEPARATION_WIDTH + WHEEL_SEPARATION_LENGTH)*angularSpeed)
+ speeds.rearRight = self.ticksPerMeter * (linearXSpeed - linearYSpeed + (WHEEL_SEPARATION_WIDTH + WHEEL_SEPARATION_LENGTH)*angularSpeed)
+
+ # print(speeds.frontLeft, speeds.frontRight, speeds.rearLeft, speeds.rearRight)
+
+
+ # Adjust speeds if they exceed the maximum.
+ if max(speeds.frontLeft, speeds.frontRight, speeds.rearLeft, speeds.rearRight) > self.maxMotorSpeed:
+ factor = self.maxMotorSpeed / max(speeds.frontLeft, speeds.frontRight, speeds.rearLeft, speeds.rearRight)
+ speeds.frontLeft *= factor
+ speeds.frontRight *= factor
+ speeds.rearLeft *= factor
+ speeds.rearRight *= factor
+
+ speeds.frontLeft = int(speeds.frontLeft)
+ speeds.frontRight = int(speeds.frontRight)
+ speeds.rearLeft = int(speeds.rearLeft)
+ speeds.rearRight = int(speeds.rearRight)
+ return speeds
+
+ def setWheelSeparation(self, separation):
+ self.wheelSeparation = separation
+
+ def setMaxMotorSpeed(self, limit):
+ self.maxMotorSpeed = limit
+
+ def setTicksPerMeter(self, ticks):
+ self.ticksPerMeter = ticks
diff --git a/src/mecanum_drive/controller.pyc b/src/mecanum_drive/controller.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..bf6c72d36464af76c9709cda0465ab1b6a10f0cd
Binary files /dev/null and b/src/mecanum_drive/controller.pyc differ
diff --git a/src/mecanum_drive/encoder.py b/src/mecanum_drive/encoder.py
new file mode 100755
index 0000000000000000000000000000000000000000..d99e59728b26f88e7844fedfcbf7f0a6aad0e65f
--- /dev/null
+++ b/src/mecanum_drive/encoder.py
@@ -0,0 +1,52 @@
+from __future__ import division
+
+
+class Encoder:
+ """Monitors a single wheel encoder and accumulates delta ticks
+ since the last time they were requested.
+ """
+
+ def __init__(self):
+ self.setRange(-32768, 32767)
+ self.initCount(0)
+ self.isReversed = False
+
+ def setRange(self, low, high):
+ self.range = high - low + 1
+ self.lowThresh = low + self.range*30//100
+ self.highThresh = low + self.range*70//100
+
+ def initCount(self, startCount):
+ self.delta = 0
+ self.last = startCount
+
+ def update(self, newCount):
+ if self.last > self.highThresh and newCount < self.lowThresh:
+ # Wrapped around the upper limit
+ increment = newCount + self.range - self.last
+ elif self.last < self.lowThresh and newCount > self.highThresh:
+ # Wrapped around the lower limit
+ increment = newCount - self.range - self.last
+ else:
+ increment = newCount - self.last
+
+ self.delta += increment
+ self.last = newCount
+
+ def setReversed(self, isReversed):
+ self.isReversed = isReversed
+
+ def getDelta(self):
+ delta = self.delta
+ self.delta = 0
+ if self.isReversed:
+ return -delta
+ else:
+ return delta
+
+ def getLimits(self):
+ return {
+ 'range': self.range,
+ 'lowThresh': self.lowThresh,
+ 'highThresh': self.highThresh
+ }
diff --git a/src/mecanum_drive/encoder.pyc b/src/mecanum_drive/encoder.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..5b0ef9b08957448661a87ccd075d53fc9a4f8427
Binary files /dev/null and b/src/mecanum_drive/encoder.pyc differ
diff --git a/src/mecanum_drive/odometry.py b/src/mecanum_drive/odometry.py
new file mode 100755
index 0000000000000000000000000000000000000000..5e36026423cfc54d2e014924fb066392098b218b
--- /dev/null
+++ b/src/mecanum_drive/odometry.py
@@ -0,0 +1,79 @@
+from __future__ import division
+from math import pi, sin, cos
+from mecanum_drive.encoder import Encoder
+from mecanum_drive.pose import Pose
+
+class Odometry:
+ """Keeps track of the current position and velocity of a
+ robot using differential drive.
+ """
+
+ def __init__(self):
+ self.leftEncoder = Encoder()
+ self.rightEncoder = Encoder()
+ self.pose = Pose()
+ self.lastTime = 0
+
+ def setWheelSeparation(self, separation):
+ self.wheelSeparation = separation
+
+ def setTicksPerMeter(self, ticks):
+ self.ticksPerMeter = ticks
+
+ def setEncoderRange(self, low, high):
+ self.leftEncoder.setRange(low, high)
+ self.rightEncoder.setRange(low, high)
+
+ def setTime(self, newTime):
+ self.lastTime = newTime
+
+ def updateLeftWheel(self, newCount):
+ self.leftEncoder.update(newCount)
+
+ def updateRightWheel(self, newCount):
+ self.rightEncoder.update(newCount)
+
+ def updatePose(self, newTime):
+ """Updates the pose based on the accumulated encoder ticks
+ of the two wheels. See https://chess.eecs.berkeley.edu/eecs149/documentation/differentialDrive.pdf
+ for details.
+ """
+ leftTravel = self.leftEncoder.getDelta() / self.ticksPerMeter
+ rightTravel = self.rightEncoder.getDelta() / self.ticksPerMeter
+ deltaTime = newTime - self.lastTime
+
+ deltaTravel = (rightTravel + leftTravel) / 2
+ deltaTheta = (rightTravel - leftTravel) / self.wheelSeparation
+
+ if rightTravel == leftTravel:
+ deltaX = leftTravel*cos(self.pose.theta)
+ deltaY = leftTravel*sin(self.pose.theta)
+ else:
+ radius = deltaTravel / deltaTheta
+
+ # Find the instantaneous center of curvature (ICC).
+ iccX = self.pose.x - radius*sin(self.pose.theta)
+ iccY = self.pose.y + radius*cos(self.pose.theta)
+
+ deltaX = cos(deltaTheta)*(self.pose.x - iccX) \
+ - sin(deltaTheta)*(self.pose.y - iccY) \
+ + iccX - self.pose.x
+
+ deltaY = sin(deltaTheta)*(self.pose.x - iccX) \
+ + cos(deltaTheta)*(self.pose.y - iccY) \
+ + iccY - self.pose.y
+
+ self.pose.x += deltaX
+ self.pose.y += deltaY
+ self.pose.theta = (self.pose.theta + deltaTheta) % (2*pi)
+ self.pose.xVel = deltaTravel / deltaTime if deltaTime > 0 else 0.
+ self.pose.yVel = 0
+ self.pose.thetaVel = deltaTheta / deltaTime if deltaTime > 0 else 0.
+
+ self.lastTime = newTime
+
+ def getPose(self):
+ return self.pose;
+
+ def setPose(self, newPose):
+ self.pose = newPose
diff --git a/src/mecanum_drive/odometry.pyc b/src/mecanum_drive/odometry.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..905fe8fea72ed0ae9e457a22a2305313fa948a06
Binary files /dev/null and b/src/mecanum_drive/odometry.pyc differ
diff --git a/src/mecanum_drive/pose.py b/src/mecanum_drive/pose.py
new file mode 100755
index 0000000000000000000000000000000000000000..a9b7f3697dee2122132df2c6f4ce0d209103a63a
--- /dev/null
+++ b/src/mecanum_drive/pose.py
@@ -0,0 +1,17 @@
+from __future__ import division
+
+class Pose:
+
+ def __init__(self):
+ self.x = 0
+ self.y = 0
+ self.theta = 0
+ self.xVel = 0
+ self.yVel = 0
+ self.thetaVel = 0
+
+
+ def __str__(self):
+ return str({'x': self.x, 'y': self.y, 'theta': self.theta,
+ 'xVel': self.xVel, 'yVel': self.yVel,
+ 'thetaVel': self.thetaVel})
diff --git a/src/mecanum_drive/pose.pyc b/src/mecanum_drive/pose.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..01e65ddc0f3632bb9b99143f2296020fce3c8e1f
Binary files /dev/null and b/src/mecanum_drive/pose.pyc differ
diff --git a/tests/test_controller.py b/tests/test_controller.py
new file mode 100755
index 0000000000000000000000000000000000000000..29c67f7c72169b0e3f44334101f5e35f5911be3d
--- /dev/null
+++ b/tests/test_controller.py
@@ -0,0 +1,66 @@
+from __future__ import division
+PKG='test_controller'
+
+import unittest
+from diff_drive.controller import Controller
+
+
+class TestController(unittest.TestCase):
+
+ def setUp(self):
+ self.ticksPerMeter = 10000
+ self.wheelSeparation = 0.1
+ self.controller = Controller()
+ self.controller.setTicksPerMeter(self.ticksPerMeter)
+ self.controller.setWheelSeparation(self.wheelSeparation)
+
+ def testStraightForward(self):
+ speeds = self.controller.getSpeeds(0.1, 0)
+ self.assertAlmostEqual(speeds.left, self.ticksPerMeter*0.1)
+ self.assertAlmostEqual(speeds.right, self.ticksPerMeter*0.1)
+
+ def testStraightBackward(self):
+ speeds = self.controller.getSpeeds(-0.1, 0)
+ self.assertAlmostEqual(speeds.left, -self.ticksPerMeter*0.1)
+ self.assertAlmostEqual(speeds.right, -self.ticksPerMeter*0.1)
+
+ def testRotateLeft(self):
+ speeds = self.controller.getSpeeds(0, 1)
+ diffTicks = self.wheelSeparation * self.ticksPerMeter
+ self.assertAlmostEqual(speeds.left, -diffTicks)
+ self.assertAlmostEqual(speeds.right, diffTicks)
+
+ def testRotateRight(self):
+ speeds = self.controller.getSpeeds(0, -1)
+ diffTicks = self.wheelSeparation * self.ticksPerMeter
+ self.assertAlmostEqual(speeds.left, diffTicks)
+ self.assertAlmostEqual(speeds.right, -diffTicks)
+
+ def testCurveLeft(self):
+ speeds = self.controller.getSpeeds(0.1, 1)
+ aheadTicks = 0.1 * self.ticksPerMeter
+ diffTicks = self.wheelSeparation * self.ticksPerMeter
+ self.assertAlmostEqual(speeds.left, aheadTicks-diffTicks)
+ self.assertAlmostEqual(speeds.right, aheadTicks+diffTicks)
+
+ def testMotorLimitsStraight(self):
+ maxTickSpeed = self.ticksPerMeter // 4
+ self.controller.setMaxMotorSpeed(maxTickSpeed)
+ speeds = self.controller.getSpeeds(1, 0)
+ self.assertEqual(speeds.left, maxTickSpeed)
+ self.assertEqual(speeds.right, maxTickSpeed)
+
+ def testMotorLimitsCurved(self):
+ maxTickSpeed = self.ticksPerMeter // 4
+ self.controller.setMaxMotorSpeed(maxTickSpeed)
+ speeds = self.controller.getSpeeds(1, 1)
+ aheadTicks = self.ticksPerMeter
+ diffTicks = self.wheelSeparation * self.ticksPerMeter
+ factor = maxTickSpeed / (aheadTicks + diffTicks)
+ self.assertEqual(speeds.left,
+ int((aheadTicks-diffTicks) * factor))
+ self.assertEqual(speeds.right, maxTickSpeed)
+
+
+if __name__ == '__main__':
+ unittest.main()
diff --git a/tests/test_encoder.py b/tests/test_encoder.py
new file mode 100755
index 0000000000000000000000000000000000000000..5923d171aeb3c6134490c7bf61064eaca5f7abc7
--- /dev/null
+++ b/tests/test_encoder.py
@@ -0,0 +1,62 @@
+#! /usr/bin/env python
+from __future__ import division
+PKG='test_encoder'
+
+import unittest
+from diff_drive.encoder import Encoder
+
+
+class TestEncoder(unittest.TestCase):
+
+ def setUp(self):
+ self.encoder = Encoder()
+
+ def testInitialization(self):
+ self.assertEquals(self.encoder.getDelta(), 0)
+
+ def testClearedDelta(self):
+ self.encoder.update(100)
+ self.assertEquals(self.encoder.getDelta(), 100)
+ self.assertEquals(self.encoder.getDelta(), 0)
+
+ def testIncrement(self):
+ self.encoder.update(100)
+ self.assertEquals(self.encoder.getDelta(), 100)
+
+ self.encoder.update(50)
+ self.assertEquals(self.encoder.getDelta(), -50)
+
+ def testWraparound(self):
+ defaultRange = 32767 - (-32768) + 1
+ self.encoder.update(20000)
+ self.assertEquals(self.encoder.getDelta(), 20000)
+
+ # Wrap around the high end.
+ self.encoder.update(-20000)
+ self.assertEquals(self.encoder.getDelta(),
+ (-20000) + defaultRange - 20000)
+
+ # Wrap around the low end.
+ self.encoder.update(20000)
+ self.assertEquals(self.encoder.getDelta(),
+ 20000 - defaultRange - (-20000))
+
+ def testCustomRange(self):
+ self.encoder.setRange(0, 999)
+ self.encoder.update(500)
+ self.assertEquals(self.encoder.getDelta(), 500)
+
+ self.encoder.update(900)
+ self.assertEquals(self.encoder.getDelta(), 400)
+
+ # Wrap around the high end.
+ self.encoder.update(100)
+ self.assertEquals(self.encoder.getDelta(), 200)
+
+ # Wrap around the low end.
+ self.encoder.update(900)
+ self.assertEquals(self.encoder.getDelta(), -200)
+
+
+if __name__ == '__main__':
+ unittest.main()
diff --git a/tests/test_odometry.py b/tests/test_odometry.py
new file mode 100755
index 0000000000000000000000000000000000000000..7f33b9975fa511a32aff21d71bc7708ace753000
--- /dev/null
+++ b/tests/test_odometry.py
@@ -0,0 +1,107 @@
+#! /usr/bin/env python
+from __future__ import division
+PKG='test_odometry'
+
+from math import pi, sin, cos
+import unittest
+from diff_drive.odometry import Odometry
+
+
+class TestOdometry(unittest.TestCase):
+
+ def setUp(self):
+ self.wheelSeparation = 0.10
+ self.ticksPerMeter = 10000
+ self.odom = Odometry()
+ self.odom.setWheelSeparation(self.wheelSeparation)
+ self.odom.setTicksPerMeter(self.ticksPerMeter)
+
+ def testInitialization(self):
+ pose = self.odom.getPose()
+ self.assertEquals(pose.x, 0)
+ self.assertEquals(pose.y, 0)
+ self.assertEquals(pose.theta, 0)
+ self.assertEquals(pose.xVel, 0)
+ self.assertEquals(pose.yVel, 0)
+ self.assertEquals(pose.thetaVel, 0)
+
+ def testTravelForward(self):
+ self.checkUpdate(10000, 10000, 2, {'x': 1, 'xVel': 1/2})
+
+ def testSpinLeft(self):
+ angle = (200/self.ticksPerMeter / self.wheelSeparation) % (2*pi)
+ self.checkUpdate(-100, 100, 2,
+ {'theta': angle, 'thetaVel': angle/2})
+
+ def testSpinRight(self):
+ angle = (200/self.ticksPerMeter / self.wheelSeparation) % (2*pi)
+ self.checkUpdate(100, -100, 2,
+ {'theta': (-angle) % (2*pi),
+ 'thetaVel': -angle/2})
+
+ def testCurveLeft(self):
+ radius = self.wheelSeparation / 2
+ angle = pi
+ s = angle * self.wheelSeparation
+ ticks = int(s * self.ticksPerMeter)
+ self.checkUpdate(0, ticks, 2,
+ {'x': 0,
+ 'y': 2*radius,
+ 'theta': angle,
+ 'xVel': s/4,
+ 'yVel': 0,
+ 'thetaVel': angle/2})
+
+ def testCurveRight(self):
+ radius = self.wheelSeparation / 2
+ angle = pi
+ s = angle * self.wheelSeparation
+ ticks = int(s * self.ticksPerMeter)
+ self.checkUpdate(ticks, 0, 2,
+ {'x': 0,
+ 'y': -2*radius,
+ 'theta': -angle,
+ 'xVel': s/4,
+ 'yVel': 0,
+ 'thetaVel': -angle/2})
+
+ def testCircle(self):
+ self.checkCircleRight(8, 9)
+ self.checkCircleRight(0, 100)
+ self.checkCircleRight(100, 0)
+
+ def checkCircle(self, vr, vl):
+ radius = abs(self.wheelSeparation/2 * (vr+vl)/(vr-vl))
+ circumference = 2*pi*radius
+ deltaTravel = (vr+vl)/2 * self.ticksPerMeter;
+ for i in range(int(circumference/deltaTravel)):
+ self.odom.updateLeftWheel(vl)
+ self.odom.updateRightWheel(vr)
+ self.odom.updatePose(i+1)
+ self.checkPose(self.odom.getPose(), {'x': 0, 'y': 0})
+
+ def checkUpdate(self, leftTicks, rightTicks, deltaTime, attrs):
+ self.odom.updateLeftWheel(leftTicks)
+ self.odom.updateRightWheel(rightTicks)
+ self.odom.updatePose(deltaTime)
+ self.checkPose(self.odom.getPose(), attrs)
+
+ def checkPose(self, pose, attrs):
+ for key in ['x', 'y', 'theta', 'xVel', 'yVel', 'thetaVel']:
+ if key in attrs:
+ self.assertClose(
+ getattr(pose, key), attrs[key],
+ msg="{0}: {1}!={2}".format(key,
+ getattr(pose, key),
+ attrs[key]))
+ else:
+ self.assertEquals(getattr(pose, key), 0, key)
+
+ def assertClose(self, x, y, msg):
+ if y == 0:
+ self.assertLess(abs(x), 0.0001, msg)
+ else:
+ self.assertLess(abs(x-y)/y, 0.001, msg)
+
+if __name__ == '__main__':
+ unittest.main()