From d289bc0ebd169bbd0dc963280be76964ba837ac0 Mon Sep 17 00:00:00 2001
From: daviddudas <david.dudas@outlook.com>
Date: Wed, 15 Jan 2020 16:58:50 +0100
Subject: [PATCH] Initial commit
---
CMakeLists.txt | 44 ++++++
LICENSE | 27 ++++
README.asciidoc | 234 +++++++++++++++++++++++++++++++
action/GoToPose.action | 9 ++
launch/demo.launch | 68 +++++++++
launch/revvy-config.launch | 42 ++++++
nodes/mecanum_drive_controller | 68 +++++++++
nodes/mecanum_drive_odometry | 123 ++++++++++++++++
package.xml | 22 +++
setup.py | 12 ++
src/mecanum_drive/__init__.py | 0
src/mecanum_drive/controller.py | 62 ++++++++
src/mecanum_drive/controller.pyc | Bin 0 -> 2858 bytes
src/mecanum_drive/encoder.py | 52 +++++++
src/mecanum_drive/encoder.pyc | Bin 0 -> 2416 bytes
src/mecanum_drive/odometry.py | 79 +++++++++++
src/mecanum_drive/odometry.pyc | Bin 0 -> 3929 bytes
src/mecanum_drive/pose.py | 17 +++
src/mecanum_drive/pose.pyc | Bin 0 -> 1015 bytes
tests/test_controller.py | 66 +++++++++
tests/test_encoder.py | 62 ++++++++
tests/test_odometry.py | 107 ++++++++++++++
22 files changed, 1094 insertions(+)
create mode 100755 CMakeLists.txt
create mode 100755 LICENSE
create mode 100755 README.asciidoc
create mode 100755 action/GoToPose.action
create mode 100755 launch/demo.launch
create mode 100755 launch/revvy-config.launch
create mode 100755 nodes/mecanum_drive_controller
create mode 100755 nodes/mecanum_drive_odometry
create mode 100755 package.xml
create mode 100755 setup.py
create mode 100755 src/mecanum_drive/__init__.py
create mode 100755 src/mecanum_drive/controller.py
create mode 100644 src/mecanum_drive/controller.pyc
create mode 100755 src/mecanum_drive/encoder.py
create mode 100644 src/mecanum_drive/encoder.pyc
create mode 100755 src/mecanum_drive/odometry.py
create mode 100644 src/mecanum_drive/odometry.pyc
create mode 100755 src/mecanum_drive/pose.py
create mode 100644 src/mecanum_drive/pose.pyc
create mode 100755 tests/test_controller.py
create mode 100755 tests/test_encoder.py
create mode 100755 tests/test_odometry.py
diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100755
index 0000000..8bafa33
--- /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 0000000..8626745
--- /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 0000000..54127dc
--- /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 0000000..1cf9b79
--- /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 0000000..60711d5
--- /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 0000000..39ab4be
--- /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 0000000..d788a3a
--- /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 0000000..74c05ac
--- /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 0000000..eb225cf
--- /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 0000000..dec9110
--- /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 0000000..e69de29
diff --git a/src/mecanum_drive/controller.py b/src/mecanum_drive/controller.py
new file mode 100755
index 0000000..c7f8be4
--- /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
GIT binary patch
literal 2858
zcmZSn%*(a)nQUA#0~9bbFfceMFfbHbF)%QsFfgPrGUPBY<T5fuF)}cwFfpXCGo&yx
zv@kF<GcZI(F)^gDFr;uWq_Dz;m>E*o7=kr885kHc|NsC0UxSf>fuV#0WLjofW^rbI
zUI`Ba1A}jVNq&)Yer|4JUJBfDkWwd*<>Cws45<ttJ5v}KK(;V~%#UJb2nyC<Vqjn>
zc4S~+@W{_eDOSh@8KjV$pI1_ppQDfrF|k-7Ex$-1Q6VKWEiJVuHLoNyF-JG0D6=e8
zp(sBozeJBqK|w(w8Dt*_gN$?r*{cC^CL==)14BI6>mUXrC=5UhCK!Vm6fh8n_+^5^
zO@jsGM@|L?hP0ymyb_<(w2~4o7%wO@J);C107a>ZMPNCQN)QJu1qmRK3B{>7X&@Ic
zfQe#v1_lQGjQreG{glMA%oP3P#FFgHy!i5B{o<lz{oK^##Jtknc(AXqh(JRmC$&hg
zpb~6-e0*kJW=VWJJ177_5yHU835F2ogGA!v^AdAY<Kw}ukB`sIPbtj-aY0(*<5TjJ
z<Ku%sY_J0n1T~!+1d0$)Dl7&C2^$j|6WB4%kO<C6Ey7GK*0@qj3KKZlut1Y+6e}pX
zwlFY6u|ZR86gwzIv4H}_hJk^>CAB2AC^s`N6_%`u3sO^4iWN%o6%vz^^K%PwGK(`n
z$ttxtvnVwMl=d?7^Kd05UX;YdzyM1z0_qG5kVprq$W5#O=Y?R9Rbb&Dkb7tpxS+@r
z0(lpd<QbTgL1E0zz`y{q5}X4rGlCO-Eek^m6GJU4LkT#i*Rp}~L>41M4I@Jh6N5wz
z3quVngLn-ygNTGk4I4u|6N<bzlDs%ro*Ab+L_Z6PJhJ%^{j3Z%Yz$dUV3HY3vVcie
zhBOuicMp)W7#Ok`8ANK?8EPPwu`!6$K*cy<-r!`YVP~k}V34Tc1iKl`5eIRY8A{l}
zCStRr8R9$+hHMswqIO1*u^a@{OV@INjAH@0nTx@a2^2zX3@lm93^iab*b^*SEKn}k
zH!NAKP%bM&9s@%OC&+{>CWdB45F?%mCc+F8VTOsYz(iPJBCIeGaL5H~fO3>y`mC8C
zu+u($Nr5fb^~DG5HMkfU7{FPs1e6#{GLy5515%58K^Y+kltzL;8iPQ|B#0GM$ni5U
zFqCJcrsf2v79<uWmVnB0kdoZQiXa}40uU!NuSA0zWH2aMfs6Z`%)HdZq6lyzM-qrc
z2qfmEm*ylEK?KAY7#PAmTwQ(QgIxn0gB(LV{r%#@JzYXPN<dWuhLn%1pL>W$3CKD~
zmIW7hAm0QfmZX+&f^uSJT3QImdnKSe08wQG){&N&T#{czn+ylGIz6=n?5$#u+lviA
zH3lmiBO4PZlL8|cGRZRYGKw&AGV(I=vS~4DF!F<;5R(ui1SW%`0*1l)6ck&KR1d1D
zK!ppa-1LJKAbboA4B&baT$F&z&tiCV(a59VQYEz{95rn?FfcF_gB;GlNTMG=i3!OM
zpv0GxnVVTcBj*c%9GY6<i(DEzgT%;kJScY191lv2#k6%iIC;ZzuN%Vg8lb8@2uy%-
zLJ&C5gK|NTG$@nGf(UsKp$JMhpyWv#cLssV1W?*621PEYYUgC-)Z}F5WcEW?2x*Lf
woERUUR$5Y8lo}r&1acF&lmZ)pq!=VY&DICF+^~VPd+b0t3fu}2U=rpA0Gk<du>b%7
literal 0
HcmV?d00001
diff --git a/src/mecanum_drive/encoder.py b/src/mecanum_drive/encoder.py
new file mode 100755
index 0000000..d99e597
--- /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
GIT binary patch
literal 2416
zcmZSn%*$nAC?1#000qnp3=9qm3=GBU3=9k@3=AoZ3^@!8xr_`^j0}t^ObjWk3@OYE
zEes6J3=EM`ObjV348a;~3=9mJ|NsC0uffQ`z)->g(wbS8S)7@lSHjM~z~GvfoS%|f
z1h)jF$_ZqNF9QQZDg(&u6b1&6X^bGFqnH^|n7|AcsAW;CAd6ZU7^2ufE&wsu8B*B5
zYB(5D*ue}=hM-^#kln?(3=9mu`FWWo`9;MFi3-J;dFeT+3gsE8sW}R%5Vt5K=A|ek
zCMTEXmgXduq!ufrq~??)DwJd<XBTrRC@6q5C8sKsWTYzOBo>z_lw{_Dd6f#~sYR&@
zMX80Qsl_F!DSBYV$siwrFevz(K|a%BU|^_WV8{Z8U<wmMGZO<Nh|dV}Q8Ob0V+j*T
zKAssA$iW&c3=9l@nG6m8|7ZTMXJE(##Rgaq9GJzaB|(XK>8T~0pn%THEOE{+%_}Kk
zWnf@%OUx-wE#U%*7YC)5r4|*Zra)o=WMpw_PMQYTED%xb!oa|wpOK%Ns-KcrmYJfT
zoLG{bnHOJPtY2J|te=~joS0Xd8=q2?S(b`L1R8RB1(je&#m8snWtPOpb22b66oXO#
z1ET;VFJm$&2w)f-6t1A4U}C6YWROT<WDu`mV2Eb`@j$`B0E%gm6lMk~us9=<I12+p
zoCy+2AP@Uxg2Fjd4&>t$5aGtaz@P!L5F8#w@Nmk>FAvEmN-fR+hfYRjdIp3GRvQFz
za0y6PPJVd_D5yXRXb@CEObiSRe25?tWRze`28qEiIEchRIffCOAYefg&j|4|NSz-f
zJ%E^CPlD4b*wvu4sR0s)cnFjZic1oUAPIyP-T?)fFvvTgFk@ikgm?xd3c_H|%ml?Z
zIO&68n2{lyg`vm-7O_kW*$hZrW^j7QVqlP{Wn##41%*HsB;!C77QpmDjDd5R86-f4
zL-p~(R6+PP3=Aw;EDSYF4CYMnEKs?4aB&t4&KrIjpp+B@ik2WykOgrvFfasxijE*&
zSZaa<C`c?XwH%V)K&mqHl8aJvQ}aq_5V$3vL|Iypl30=|4hmwB`xqD%nZ!YuiI<5V
z64;>V24QeugWOxg2nsh;4}vmD5XhH^)B)jw3Ied(saG0sgB+V$0xN!0KwOXm85qIk
zB8UmXVE2NohQwSgBdBtLmIe$AS&R%t3ZS^+4z6Kj2u@*O2nIVHDeZ$J2jUBmdEgiV
zi-8Pf029>n6e#A?Q%hVx)dHw4ECwYMP=e=V<YZ)J0{aXU$na#Z1S*V}K`{X-^vyut
zWCWFOU=|ao;5UPo^pLd12}*S!V<ANn$VNy?1dD+T2YV6hF)%?r-+`=7Pc8Au%*`w*
z)(1rx$aV%sW=1YXE=CQI1E7U`USe))d_1@ejgQaGPbtj-aY40ue0)lNa(sLchz&00
zf<R#rBn0vmsE7y>0kK3u848~0sOya&P#A#<qGACC1_m}}HYQG1PF7A9PF_xCP7Xh?
uW#F)dIzO$nq_ikCK0XNK9<XMJ5)c>6qFw-lg3|^PTy`K=7K2i(05bqjnET)W
literal 0
HcmV?d00001
diff --git a/src/mecanum_drive/odometry.py b/src/mecanum_drive/odometry.py
new file mode 100755
index 0000000..5e36026
--- /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
GIT binary patch
literal 3929
zcmZSn%*%EAtZZB|0~9bbFfceMFfbH@L{b<SQWzO>7#MOH8KM{&7(sj{h8!k_TxNzS
zW`<lAhA0+>TvmoCR)`!kLk=55E;~aMJA}`|ki)@{%gGSM$-tPx%8<grkiy2$!obkX
zzz`Y5#gM|z5Ujz;z`&6C|NsC08jK7K3?&>43=AomWtqj9`FR>3$CNNJFfbHkmVnsB
znR#G3Ilma9nw^1x!8I>AKP9yYBEZ7Hzz~pMoC-F_KP5jmwWO#L<Q1^@KsGpmJeR@1
zz>vxS@>2=}LlhIpmn{q+FQqVn+!w{dkiraRurj2ufEjEIDXd@yJJb(R93c0Dba8?_
z24ZkAq;P`Oa5JQEff+muLBSf_3=9m#aSRL$-l?et#R?@wiOJat`DqFz8L0}%rA0-l
zc_j)3`Nf$fnfZANiFqjsWvMy&$(bdUAVrB>3JMAeMfpkjB?_g*nR)37DVb?$sUS_6
zi8%@>MVV!(dSErlAm4#7D8QUSzBB{{C^)oh7#QLi8A{-6CWaDbP%MDNSwOxAF<2Q=
z7#M;zK-&E>LE)vr#=yW31Pam;kRNhV(@LO$UBbh_z)+N#o&ggGVg=a+Qd$5D{hY+&
zl90^YR7AuSr{<(-fDHx_#jXqt4Eh=QxvBaoiDj87`pJnU*_nCq<;D8NMalZPsmY0X
zrMdB7A7T;7heobmK_xh{<Kr{)GE3s)K`t!@r6LAK9xzM>c?gEV;lj(nz)-`;0P-^f
z#Lu8$^Mg2)pMim)JR>zVCpfhru_zG~Dc~Rp0vTMw1&T|A1jrv?A5qaWLSQRO!cpBI
z2oeSPf`O4l4}pAJl9`-c9FSV%n_7}ugybD?tbydfo&Xb6^oan-&8a0JAPZoYi-5$S
z{s3iZ7zRg-7ARsE7_t}`vKYZJ#Kh3d#K2g?z>vj+Dg;ickXQqm5Ckd+z{#LEwInDp
zFFiE~lmx&92qfY_s&ewnOF&69BQre%>{l>BMGu40PH`$cPk`JZ4)QO^QU*q05@Rk1
zl-Q6W3*^VV)N*j1qD8_3nO_WwuG~~<kgK7w1fpOV97~`m1VsrfmYN|oLohi1_(2>D
zGOGkse3TZXB$lK?1i?876fb$H<<9w~c_rZR2NP6|1c<FZscDdGBuA-tK#mTQWME)G
zcqB*)#HB?%fU*t513{n?7F-Z0qWJ(+{(@2!I70p~GJpy*NVaK)u$dU7Yni~J%%Gfu
zB+3lVSquzWEDRF0EDSZw3^hy);wg*_(zUEmo&+d*SsA2j*`Na1ObkVnLF!7_z-F>D
zWN|PwGct(OvV%oA(L^~I@=h_-urk!JF-X_KCAk=CI2lUdT!;x0wOlaG++cY$8^mk5
z89=tOf$Rghi=82hi=l>#K|%y%6tXZkLk%~Bgos297lSxlr9>?|*iK}HF#T*G2R4Ii
zf-H81EU13*8g96LZU%{34ygNBK=!gTm@~!0T+9N}!T}QDhN{Wp02v7i%@Q645iy7>
z;yD;<SQt{67_wOyincO>!i9xFI*%FD$bhQg1xZ8Md`PMvGuE(yxTvc6p-~vm0&1QF
zYk-RJ;sXo}458rMRt#=hfGT2zq{QOX6ovdeut;KZa%pa9PGU)Fib5)+)uaHf8Nlra
zP&)(KDk&+?R{+<6#d-?Csi_JXB_##LR{HwM8L7p^da0?&#d=ApMcJu2sg-)EDW&=#
z5knJ8{gnLV(%jU%5^$}lkK9mk0k=~MQqtg7rR5hXq@<Q4X66(_+AkpcGeHFnxb2Y+
z%2^=nry&4pg@6kbP&Q0YEpbWBDM<_h3xh<!<zx`3nhOGz@<E)SrU)oVN-|PQ5`#cS
zmVl~?3NTq21Y(2hfr_xy95AC2!~huu))AJPqXEia;EE#%RDgr4eNgi!q$sg0H3!@p
z0=Iske2|SPAV-19X0R-T2T{!hYSMybLGA&UieTvo1TzxMEJ{qtEG-7RC^I=Z0>+30
zmylqBip3<zRgh8!)LvErl`mi`Sj8B{7<rla7zLSl7{wSRnYfr07!_C?SR9yy7=;++
z7?l`#7?qgdtxZs9f=eM#41rQGs0|4cgtVu?u>r0bz`+T&0!&cRD<GTGQ%gYp@CNw<
z6yu~elOXN{=>)s81Y~AjYB|VEuyer#mEBwnakC%7%^ILo3~tB7$LA&HrpCvElU{s$
zZhlH>4u}hKSA2X*esX+#5IB*7+8jYbpe!N=A|yZrsQM0)0kQNzgg%G>bq2tmKoHdP
zP7tW72jOCnBiNYPm^j%unK_v`S->PSr#dIIAH-sCBLI~6;^WgwOG=AU<Ku%s`5c_u
za}!H4!1)uLYr)yMM3jMn0i`pb2Q9lGJrj`EQKdlzAGkjPb```{kPToK_52KuCL2h^
T*ntu}xYr@TD8negD8K{&^4(ZH
literal 0
HcmV?d00001
diff --git a/src/mecanum_drive/pose.py b/src/mecanum_drive/pose.py
new file mode 100755
index 0000000..a9b7f36
--- /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
GIT binary patch
literal 1015
zcmZSn%*$nAC?1#000qnp3=9qm3=GBU3=9k@3=AoZ3^@!8xr_`^j0}t^ObjWk3@OYE
zEes6J3=EM`ObjV348a;~3=9mJ|NsC0uffQ`z)->g(wbS8S)7@lSHi-;zz~pMoC>!A
zq{InigA@Y;Ln;Hv;1mXsi6GM$!3<`GpkNIq1_p*?kYW%9ahyS#tUzWnGSo0I#Dg6N
zVlaYS31Tq87|bvR3yi@E@(jdKzf6!{!HzBgnOy-UD@#~GZqG<9Nd&vOA}lor%%}u0
zKwc^V$%UonKs*WZS8-}ing-Y~5K-*Nz`&rNk)NBYpORRXnWCSZSdyKY7hhhiUtE-|
zpPQPTm{*z`pHh@rmWoBBAip?Oub>hfdhzj@d6^~g@gOG^^MDLz<b*?bpn#kJ4iq<#
z%NZCl*+Aiv1r83g8U}_eCWaIy1~7{m6l-7>3n<FKELKotf>~?~DQpa8&5R7e8te=V
z41PhN;0^*Mg&>d}K_F)Yfg(8wBn?(m0+KB*DI%a06uv>Az@vIFfm{+FUtCfYAJ50Y
zz)%cw9s{EaqX?r0$j#7T%uCEojgK$kVqjp1kI&6dDa}cZj}PKxU|<O1W?*1IxC+do
zP7DTtJPi&WkVDv*I63{mrfY!2piW6EEh#MmIRzBAU=0u@ATF3i9jAap!3N@QJCL)A
JK`|@91OQV*%BKJT
literal 0
HcmV?d00001
diff --git a/tests/test_controller.py b/tests/test_controller.py
new file mode 100755
index 0000000..29c67f7
--- /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 0000000..5923d17
--- /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 0000000..7f33b99
--- /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()
--
GitLab