├── src
    └── diff_drive
    │   ├── __init__.py
    │   ├── pose.py
    │   ├── mock_robot.py
    │   ├── controller.py
    │   ├── encoder.py
    │   ├── odometry.py
    │   └── goal_controller.py
├── action
    └── GoToPose.action
├── urdf
    └── diff_drive_description.xacro
├── setup.py
├── package.xml
├── .gitignore
├── CMakeLists.txt
├── LICENSE
├── tests
    ├── test_mock_robot.py
    ├── test_encoder.py
    ├── test_controller.py
    ├── test_odometry.py
    └── test_goal_controller.py
├── nodes
    ├── diff_drive_mock_robot
    ├── diff_drive_controller
    ├── diff_drive_odometry
    └── diff_drive_go_to_goal
├── launch
    └── demo.launch
├── config
    └── view.rviz
└── README.asciidoc


/src/diff_drive/__init__.py:
--------------------------------------------------------------------------------
1 | 


--------------------------------------------------------------------------------
/action/GoToPose.action:
--------------------------------------------------------------------------------
 1 | # goal definition
 2 | geometry_msgs/PoseStamped pose
 3 | ---
 4 | # result definition
 5 | bool success
 6 | ---
 7 | # feedback
 8 | bool processing
 9 | 
10 | 


--------------------------------------------------------------------------------
/urdf/diff_drive_description.xacro:
--------------------------------------------------------------------------------
1 | <?xml version="1.0" encoding="UTF-8" ?>
2 | <robot name="mock_robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
3 | 
4 |   <!-- The robot link description -->
5 |   <link name="base_link" />
6 | 
7 | </robot>
8 | 


--------------------------------------------------------------------------------
/setup.py:
--------------------------------------------------------------------------------
 1 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
 2 | 
 3 | from distutils.core import setup
 4 | from catkin_pkg.python_setup import generate_distutils_setup
 5 | 
 6 | # fetch values from package.xml
 7 | setup_args = generate_distutils_setup(
 8 |     packages=['diff_drive'],
 9 |     package_dir={'': 'src'})
10 | 
11 | setup(**setup_args)
12 | 
13 | 


--------------------------------------------------------------------------------
/src/diff_drive/pose.py:
--------------------------------------------------------------------------------
 1 | from __future__ import division
 2 | 
 3 | class Pose:
 4 | 
 5 |     def __init__(self):
 6 |         self.x = 0
 7 |         self.y = 0
 8 |         self.theta = 0
 9 |         self.xVel = 0
10 |         self.yVel = 0
11 |         self.thetaVel = 0
12 | 
13 | 
14 |     def __str__(self):
15 |         return str({'x': self.x, 'y': self.y, 'theta': self.theta,
16 |                     'xVel': self.xVel, 'yVel': self.yVel,
17 |                     'thetaVel': self.thetaVel})
18 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0" encoding="utf-8"?>
 2 | <package format="2">
 3 |   <name>diff_drive</name>
 4 |   <version>1.0.0</version>
 5 |   <description>
 6 |     This package implements a control mechanism for a differential drive robot.
 7 |   </description>
 8 |   <author>Mark Rose</author>
 9 |   <maintainer email="markrose@acm.org">Mark Rose</maintainer>
10 |   <url type="website">https://github.com/merose/diff_drive</url>
11 |   <url type="repository">https://github.com/merose/diff_drive.git</url>
12 |   <url type="bugtracker">https://github.com/merose/diff_drive/issues</url>
13 |   <license>BSD</license>
14 |   <buildtool_depend>catkin</buildtool_depend>
15 | 
16 |   <depend>std_msgs</depend>
17 |   <depend>geometry_msgs</depend>
18 |   <depend>actionlib_msgs</depend>
19 |   <build_depend>rostest</build_depend>
20 |   <depend>message_generation</depend>
21 |   
22 | </package>
23 | 


--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
 1 | # Byte-compiled / optimized / DLL files
 2 | __pycache__/
 3 | *.py[cod]
 4 | *$py.class
 5 | 
 6 | # C extensions
 7 | *.so
 8 | 
 9 | # Distribution / packaging
10 | .Python
11 | env/
12 | build/
13 | develop-eggs/
14 | dist/
15 | downloads/
16 | eggs/
17 | .eggs/
18 | lib/
19 | lib64/
20 | parts/
21 | sdist/
22 | var/
23 | *.egg-info/
24 | .installed.cfg
25 | *.egg
26 | 
27 | # PyInstaller
28 | #  Usually these files are written by a python script from a template
29 | #  before PyInstaller builds the exe, so as to inject date/other infos into it.
30 | *.manifest
31 | *.spec
32 | 
33 | # Installer logs
34 | pip-log.txt
35 | pip-delete-this-directory.txt
36 | 
37 | # Unit test / coverage reports
38 | htmlcov/
39 | .tox/
40 | .coverage
41 | .coverage.*
42 | .cache
43 | nosetests.xml
44 | coverage.xml
45 | *,cover
46 | .hypothesis/
47 | 
48 | # Translations
49 | *.mo
50 | *.pot
51 | 
52 | # Django stuff:
53 | *.log
54 | 
55 | # Sphinx documentation
56 | docs/_build/
57 | 
58 | # PyBuilder
59 | target/
60 | 
61 | #Ipython Notebook
62 | .ipynb_checkpoints
63 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(diff_drive)
 3 | 
 4 | find_package(catkin REQUIRED COMPONENTS
 5 |   rospy
 6 |   std_msgs
 7 |   nav_msgs
 8 |   geometry_msgs
 9 |   tf
10 |   tf2
11 |   actionlib
12 |   actionlib_msgs
13 | )
14 | 
15 | add_action_files(
16 |   DIRECTORY action
17 |   FILES GoToPose.action
18 | )
19 | 
20 | if (CATKIN_ENABLE_TESTING)
21 |   catkin_add_nosetests(tests)
22 | endif()
23 | 
24 | if (CATKIN_ENABLE_TESTING)
25 |   find_package(rostest REQUIRED)
26 | #  add_rostest(tests/your_first_rostest.test)
27 | #  add_rostest(tests/your_second_rostest.test)
28 | endif()
29 | 
30 | catkin_install_python(
31 |     PROGRAMS
32 |         nodes/diff_drive_odometry
33 |         nodes/diff_drive_controller
34 |         nodes/diff_drive_go_to_goal
35 | 	nodes/diff_drive_mock_robot
36 |     DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
37 | 
38 | catkin_python_setup()
39 | 
40 | generate_messages (
41 |   DEPENDENCIES actionlib_msgs std_msgs geometry_msgs
42 | )
43 | 
44 | catkin_package(
45 |   CATKIN_DEPENDS actionlib_msgs
46 | )
47 | 


--------------------------------------------------------------------------------
/src/diff_drive/mock_robot.py:
--------------------------------------------------------------------------------
 1 | from __future__ import division
 2 | 
 3 | 
 4 | class MotorTicks:
 5 | 
 6 |     def __init__(self):
 7 |         self.left = 0
 8 |         self.right = 0
 9 | 
10 | class MockRobot:
11 |     """Implements a mock robot that dutifully executes its wheel
12 |     speed commands exactly.
13 |     """
14 | 
15 |     def __init__(self):
16 |         self.leftSpeed = 0
17 |         self.newLeftSpeed = 0
18 |         self.rightSpeed = 0
19 |         self.newRightSpeed = 0
20 |         self.leftTicks = 0
21 |         self.rightTicks = 0
22 |         self.minTicks = -32768
23 |         self.maxTicks = 32767
24 | 
25 |     def setSpeeds(self, left, right):
26 |         self.newLeftSpeed = left
27 |         self.newRightSpeed = right
28 | 
29 |     def updateRobot(self, dTime):
30 |         self.leftTicks = self.addTicks(self.leftTicks, self.leftSpeed*dTime)
31 |         self.rightTicks = self.addTicks(self.rightTicks, self.rightSpeed*dTime)
32 |         self.leftSpeed = self.newLeftSpeed
33 |         self.rightSpeed = self.newRightSpeed
34 | 
35 |     def getTicks(self):
36 |         ticks = MotorTicks()
37 |         ticks.left = self.leftTicks
38 |         ticks.right = self.rightTicks
39 |         return ticks
40 | 
41 |     def addTicks(self, ticks, deltaTicks):
42 |         ticks += deltaTicks
43 |         if ticks > self.maxTicks:
44 |             return int(ticks - self.maxTicks + self.minTicks)
45 |         elif ticks < self.minTicks:
46 |             return int(ticks - self.minTicks + self.maxTicks)
47 |         else:
48 |             return int(ticks)
49 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | Copyright (c) 2016, Mark Rose
 2 | All rights reserved.
 3 | 
 4 | Redistribution and use in source and binary forms, with or without
 5 | modification, are permitted provided that the following conditions are met:
 6 | 
 7 | * Redistributions of source code must retain the above copyright notice, this
 8 |   list of conditions and the following disclaimer.
 9 | 
10 | * Redistributions in binary form must reproduce the above copyright notice,
11 |   this list of conditions and the following disclaimer in the documentation
12 |   and/or other materials provided with the distribution.
13 | 
14 | * Neither the name of diff_drive nor the names of its contributors may
15 |   be used to endorse or promote products derived from this software
16 |   without specific prior written permission.
17 | 
18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 | 


--------------------------------------------------------------------------------
/src/diff_drive/controller.py:
--------------------------------------------------------------------------------
 1 | from __future__ import division
 2 | 
 3 | class MotorCommand:
 4 |     """Holds motor control commands for a differential-drive robot.
 5 |     """
 6 | 
 7 |     def __init__(self):
 8 |         self.left = 0
 9 |         self.right = 0
10 |         
11 | 
12 | class Controller:
13 |     """Determines motor speeds to accomplish a desired motion.
14 |     """
15 | 
16 |     def __init__(self):
17 |         # Set the max motor speed to a very large value so that it
18 |         # is, essentially, unbound.
19 |         self.maxMotorSpeed = 1E9 # ticks/s
20 | 
21 |     def getSpeeds(self, linearSpeed, angularSpeed):
22 |         tickRate = linearSpeed*self.ticksPerMeter
23 |         diffTicks = angularSpeed*self.wheelSeparation*self.ticksPerMeter
24 | 
25 |         speeds = MotorCommand()
26 |         speeds.left = tickRate - diffTicks
27 |         speeds.right = tickRate + diffTicks
28 | 
29 |         # Adjust speeds if they exceed the maximum.
30 |         if max(abs(speeds.left), abs(speeds.right)) > self.maxMotorSpeed:
31 |             factor = self.maxMotorSpeed / max(speeds.left, speeds.right)
32 |             speeds.left *= factor
33 |             speeds.right *= factor
34 | 
35 |         speeds.left = int(speeds.left)
36 |         speeds.right = int(speeds.right)
37 |         return speeds
38 | 
39 |     def setWheelSeparation(self, separation):
40 |         self.wheelSeparation = separation
41 | 
42 |     def setMaxMotorSpeed(self, limit):
43 |         self.maxMotorSpeed = limit
44 | 
45 |     def setTicksPerMeter(self, ticks):
46 |         self.ticksPerMeter = ticks
47 | 


--------------------------------------------------------------------------------
/src/diff_drive/encoder.py:
--------------------------------------------------------------------------------
 1 | from __future__ import division
 2 | 
 3 | 
 4 | class Encoder:
 5 |     """Monitors a single wheel encoder and accumulates delta ticks
 6 |     since the last time they were requested.
 7 |     """
 8 | 
 9 |     def __init__(self):
10 |         self.setRange(-32768, 32767)
11 |         self.initCount(0)
12 |         self.isReversed = False
13 | 
14 |     def setRange(self, low, high):
15 |         self.range = high - low + 1
16 |         self.lowThresh = low + self.range*30//100
17 |         self.highThresh = low + self.range*70//100
18 | 
19 |     def initCount(self, startCount):
20 |         self.delta = 0
21 |         self.last = startCount
22 | 
23 |     def update(self, newCount):
24 |         if self.last > self.highThresh and newCount < self.lowThresh:
25 |             # Wrapped around the upper limit
26 |             increment = newCount + self.range - self.last
27 |         elif self.last < self.lowThresh and newCount > self.highThresh:
28 |             # Wrapped around the lower limit
29 |             increment = newCount - self.range - self.last
30 |         else:
31 |             increment = newCount - self.last
32 | 
33 |         self.delta += increment
34 |         self.last = newCount
35 |         
36 |     def setReversed(self, isReversed):
37 |         self.isReversed = isReversed
38 | 
39 |     def getDelta(self):
40 |         delta = self.delta
41 |         self.delta = 0
42 |         if self.isReversed:
43 |             return -delta
44 |         else:
45 |             return delta
46 | 
47 |     def getLimits(self):
48 |         return {
49 |             'range': self.range,
50 |             'lowThresh': self.lowThresh,
51 |             'highThresh': self.highThresh
52 |         }
53 | 


--------------------------------------------------------------------------------
/tests/test_mock_robot.py:
--------------------------------------------------------------------------------
 1 | from __future__ import division
 2 | PKG='test_controller'
 3 | 
 4 | import unittest
 5 | from diff_drive.mock_robot import MockRobot
 6 | 
 7 | 
 8 | class TestMockRobot(unittest.TestCase):
 9 | 
10 |     def setUp(self):
11 |         self.robot = MockRobot()
12 | 
13 |     def testNoMotion(self):
14 |         self.robot.updateRobot(1)
15 |         ticks = self.robot.getTicks()
16 |         self.assertEquals(ticks.left, 0)
17 |         self.assertEquals(ticks.right, 0)
18 | 
19 |         self.robot.updateRobot(1)
20 |         ticks = self.robot.getTicks()
21 |         self.assertEquals(ticks.left, 0)
22 |         self.assertEquals(ticks.right, 0)
23 | 
24 |     def testStraightLine(self):
25 |         self.robot.setSpeeds(100, 100)
26 |         self.robot.updateRobot(1)
27 |         ticks = self.robot.getTicks()
28 |         self.assertEquals(ticks.left, 0)
29 |         self.assertEquals(ticks.right, 0)
30 | 
31 |         self.robot.updateRobot(1)
32 |         ticks = self.robot.getTicks()
33 |         self.assertEquals(ticks.left, 100)
34 |         self.assertEquals(ticks.right, 100)
35 | 
36 |         self.robot.updateRobot(0.1)
37 |         ticks = self.robot.getTicks()
38 |         self.assertEquals(ticks.left, 110)
39 |         self.assertEquals(ticks.right, 110)
40 | 
41 |     def testRotateLeft(self):
42 |         self.robot.setSpeeds(-100, 100)
43 |         self.robot.updateRobot(0.1)
44 | 
45 |         self.robot.updateRobot(0.1)
46 |         ticks = self.robot.getTicks()
47 |         self.assertEquals(ticks.left, -10)
48 |         self.assertEquals(ticks.right, 10)
49 | 
50 |         self.robot.updateRobot(0.1)
51 |         ticks = self.robot.getTicks()
52 |         self.assertEquals(ticks.left, -20)
53 |         self.assertEquals(ticks.right, 20)
54 | 
55 |         
56 | if __name__ == '__main__':
57 |     unittest.main()
58 | 


--------------------------------------------------------------------------------
/tests/test_encoder.py:
--------------------------------------------------------------------------------
 1 | #! /usr/bin/env python
 2 | from __future__ import division
 3 | PKG='test_encoder'
 4 | 
 5 | import unittest
 6 | from diff_drive.encoder import Encoder
 7 | 
 8 | 
 9 | class TestEncoder(unittest.TestCase):
10 | 
11 |     def setUp(self):
12 |         self.encoder = Encoder()
13 |         
14 |     def testInitialization(self):
15 |         self.assertEquals(self.encoder.getDelta(), 0)
16 | 
17 |     def testClearedDelta(self):
18 |         self.encoder.update(100)
19 |         self.assertEquals(self.encoder.getDelta(), 100)
20 |         self.assertEquals(self.encoder.getDelta(), 0)
21 | 
22 |     def testIncrement(self):
23 |         self.encoder.update(100)
24 |         self.assertEquals(self.encoder.getDelta(), 100)
25 | 
26 |         self.encoder.update(50)
27 |         self.assertEquals(self.encoder.getDelta(), -50)
28 | 
29 |     def testWraparound(self):
30 |         defaultRange = 32767 - (-32768) + 1
31 |         self.encoder.update(20000)
32 |         self.assertEquals(self.encoder.getDelta(), 20000)
33 | 
34 |         # Wrap around the high end.
35 |         self.encoder.update(-20000)
36 |         self.assertEquals(self.encoder.getDelta(),
37 |                           (-20000) + defaultRange - 20000)
38 | 
39 |         # Wrap around the low end.
40 |         self.encoder.update(20000)
41 |         self.assertEquals(self.encoder.getDelta(),
42 |                           20000 - defaultRange - (-20000))
43 | 
44 |     def testCustomRange(self):
45 |         self.encoder.setRange(0, 999)
46 |         self.encoder.update(500)
47 |         self.assertEquals(self.encoder.getDelta(), 500)
48 | 
49 |         self.encoder.update(900)
50 |         self.assertEquals(self.encoder.getDelta(), 400)
51 | 
52 |         # Wrap around the high end.
53 |         self.encoder.update(100)
54 |         self.assertEquals(self.encoder.getDelta(), 200)
55 | 
56 |         # Wrap around the low end.
57 |         self.encoder.update(900)
58 |         self.assertEquals(self.encoder.getDelta(), -200)
59 | 
60 | 
61 | if __name__ == '__main__':
62 |     unittest.main()
63 | 


--------------------------------------------------------------------------------
/nodes/diff_drive_mock_robot:
--------------------------------------------------------------------------------
 1 | #! /usr/bin/env python
 2 | from __future__ import division
 3 | 
 4 | import rospy
 5 | from math import pi, asin
 6 | from geometry_msgs.msg import Twist, Pose
 7 | from nav_msgs.msg import Odometry
 8 | from std_msgs.msg import Int32
 9 | 
10 | from diff_drive import mock_robot
11 | 
12 | class MockRobotNode:
13 | 
14 |     def __init__(self):
15 |         self.robot = mock_robot.MockRobot()
16 |         self.leftSpeed = 0
17 |         self.rightSpeed = 0
18 | 
19 |     def main(self):
20 |         rospy.init_node('diff_drive_mock_robot')
21 |         self.leftPub = rospy.Publisher('~lwheel_ticks',
22 |                                        Int32, queue_size=10)
23 |         self.rightPub = rospy.Publisher('~rwheel_ticks',
24 |                                         Int32, queue_size=10)
25 | 
26 |         self.nodeName = rospy.get_name()
27 |         rospy.loginfo("{0} started".format(self.nodeName))
28 | 
29 |         rospy.Subscriber('~lwheel_desired_rate', Int32, self.leftCallback)
30 |         rospy.Subscriber('~rwheel_desired_rate', Int32,
31 |                          self.rightCallback)
32 | 
33 |         self.rate = rospy.get_param('~rate', 10.0)
34 |         self.timeout = rospy.get_param('~timeout', 0.5)
35 | 
36 |         rate = rospy.Rate(self.rate)
37 |         self.lastTime = rospy.get_time()
38 |         while not rospy.is_shutdown():
39 |             self.publish()
40 |             rate.sleep()
41 | 
42 |     def publish(self):
43 |         newTime = rospy.get_time()
44 |         diffTime = newTime - self.lastTime
45 |         self.lastTime = newTime
46 | 
47 |         if diffTime > self.timeout:
48 |             self.robot.setSpeeds(0, 0)
49 |     
50 |         try:
51 |             self.robot.updateRobot(diffTime)
52 |         except:
53 |             rospy.logerror("Got exception updating robot")
54 |         ticks = self.robot.getTicks()
55 |         self.leftPub.publish(ticks.left)
56 |         self.rightPub.publish(ticks.right)
57 | 
58 |     def leftCallback(self, leftSpeed):
59 |         self.leftSpeed = leftSpeed.data
60 |         self.robot.setSpeeds(self.leftSpeed, self.rightSpeed)
61 | 
62 |     def rightCallback(self, rightSpeed):
63 |         self.rightSpeed = rightSpeed.data
64 |         self.robot.setSpeeds(self.leftSpeed, self.rightSpeed)
65 | 
66 | 
67 | if __name__ == '__main__':
68 |     try:
69 |         node = MockRobotNode()
70 |         node.main()
71 |     except rospy.ROSInterruptException:
72 |         pass
73 | 


--------------------------------------------------------------------------------
/nodes/diff_drive_controller:
--------------------------------------------------------------------------------
 1 | #! /usr/bin/env python
 2 | from __future__ import division
 3 | 
 4 | import rospy
 5 | from geometry_msgs.msg import Twist
 6 | from std_msgs.msg import Int32
 7 | 
 8 | from diff_drive import controller
 9 | 
10 | class ControllerNode:
11 | 
12 |     def __init__(self):
13 |         self.controller = controller.Controller()
14 |         self.linearVelocity = 0.0
15 |         self.angularVelocity = 0.0
16 | 
17 |     def main(self):
18 |         self.leftPub = rospy.Publisher('lwheel_desired_rate',
19 |                                        Int32, queue_size=1)
20 |         self.rightPub = rospy.Publisher('rwheel_desired_rate',
21 |                                         Int32, queue_size=1)
22 | 
23 |         rospy.init_node('diff_drive_controller')
24 |         self.nodeName = rospy.get_name()
25 |         rospy.loginfo("{0} started".format(self.nodeName))
26 | 
27 |         rospy.Subscriber("cmd_vel", Twist, self.twistCallback)
28 | 
29 |         self.ticksPerMeter = float(rospy.get_param('~ticks_per_meter'))
30 |         self.wheelSeparation = float(rospy.get_param('~wheel_separation'))
31 |         self.maxMotorSpeed = int(rospy.get_param('~max_motor_speed'))
32 |         self.rate = float(rospy.get_param('~rate', 10.0))
33 |         self.timeout = float(rospy.get_param('~timeout', 0.2))
34 | 
35 |         self.controller.setWheelSeparation(self.wheelSeparation)
36 |         self.controller.setTicksPerMeter(self.ticksPerMeter)
37 |         self.controller.setMaxMotorSpeed(self.maxMotorSpeed)
38 | 
39 |         rate = rospy.Rate(self.rate)
40 |         self.lastTwistTime = rospy.get_time()
41 |         while not rospy.is_shutdown():
42 |             self.publish()
43 |             rate.sleep()
44 | 
45 |     def publish(self):
46 |         if rospy.get_time() - self.lastTwistTime < self.timeout:
47 |             speeds = self.controller.getSpeeds(self.linearVelocity,
48 |                                                self.angularVelocity)
49 |             self.leftPub.publish(int(speeds.left))
50 |             self.rightPub.publish(int(speeds.right))
51 |         else:
52 |             self.leftPub.publish(0)
53 |             self.rightPub.publish(0)
54 | 
55 |     def twistCallback(self, twist):
56 |         self.linearVelocity = twist.linear.x
57 |         self.angularVelocity = twist.angular.z
58 |         self.lastTwistTime = rospy.get_time()
59 | 
60 | if __name__ == '__main__':
61 |     try:
62 |         node = ControllerNode()
63 |         node.main()
64 |     except rospy.ROSInterruptException:
65 |         pass
66 | 


--------------------------------------------------------------------------------
/launch/demo.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 
 3 |   <arg name="ticks_per_meter" value="10000" />
 4 |   <arg name="wheel_separation" value="0.2" />
 5 | 
 6 |   <arg name="urdf_file" default="$(find diff_drive)/urdf/diff_drive_description.xacro" />
 7 |   <param name="robot_description" command="$(find xacro)/xacro --inorder $(arg urdf_file)" />
 8 | 
 9 |   <!-- Publish the robot state -->
10 |   <node name="robot_state_publisher" pkg="robot_state_publisher"
11 |         type="robot_state_publisher">
12 |     <param name="publish_frequency" value="10.0"/>
13 |   </node>
14 | 
15 |   <!-- Provide simulated control of the robot joint angles -->
16 |   <node name="joint_state_publisher" pkg="joint_state_publisher"
17 |         type="joint_state_publisher">
18 |     <param name="use_gui" value="False" />
19 |     <param name="rate" value="10.0"/>
20 |   </node>
21 | 
22 |   <node name="controller" pkg="diff_drive" type="diff_drive_controller"
23 |         output="screen">
24 |     <rosparam subst_value="true">
25 |       ticks_per_meter: $(arg ticks_per_meter)
26 |       wheel_separation: $(arg wheel_separation)
27 |       max_motor_speed: 3000
28 |       timeout: 1.0
29 |     </rosparam>
30 |     <remap from="cmd_vel" to="/robot/cmd_vel" />
31 |   </node>
32 |   
33 |   <node name="odom_publisher" pkg="diff_drive" type="diff_drive_odometry"
34 |         output="screen">
35 |     <rosparam subst_value="true">
36 |       ticks_per_meter: $(arg ticks_per_meter)
37 |       wheel_separation: $(arg wheel_separation)
38 |     </rosparam>
39 |   </node>
40 | 
41 |   <node name="robot" pkg="diff_drive" type="diff_drive_mock_robot"
42 |         output="screen">
43 |     <remap from="~lwheel_desired_rate" to="lwheel_desired_rate" />
44 |     <remap from="~rwheel_desired_rate" to="rwheel_desired_rate" />
45 |     <remap from="~lwheel_ticks" to="lwheel_ticks" />
46 |     <remap from="~rwheel_ticks" to="rwheel_ticks" />
47 |   </node>
48 | 
49 |   <node name="diff_drive_go_to_goal" pkg="diff_drive"
50 |         type="diff_drive_go_to_goal" output="screen">
51 |     <param name="~rate" value="20" />
52 |     <param name="~kP" value="0.5" />
53 |     <param name="~kA" value="1.0" />
54 |     <param name="~kB" value="-0.8" />
55 |     <param name="~max_linear_speed" value="0.2" />
56 |     <param name="~min_linear_speed" value="0.05" />
57 |     <param name="~max_angular_speed" value="0.7" />
58 |     <param name="~min_angular_speed" value="0.1" />
59 |     <param name="~linear_tolerance" value="0.01" />
60 |     <param name="~angular_tolerance" value="0.04" />
61 |     <param name="~forwardMovementOnly" value="false" />
62 |     <remap from="cmd_vel" to="/robot/cmd_vel" />
63 |   </node>
64 | 
65 |   <node name="rviz" pkg="rviz" type="rviz"
66 |         args="-d $(find diff_drive)/config/view.rviz" />
67 | 
68 | </launch>
69 | 


--------------------------------------------------------------------------------
/tests/test_controller.py:
--------------------------------------------------------------------------------
 1 | from __future__ import division
 2 | PKG='test_controller'
 3 | 
 4 | import unittest
 5 | from diff_drive.controller import Controller
 6 | 
 7 | 
 8 | class TestController(unittest.TestCase):
 9 | 
10 |     def setUp(self):
11 |         self.ticksPerMeter = 10000
12 |         self.wheelSeparation = 0.1
13 |         self.controller = Controller()
14 |         self.controller.setTicksPerMeter(self.ticksPerMeter)
15 |         self.controller.setWheelSeparation(self.wheelSeparation)
16 | 
17 |     def testStraightForward(self):
18 |         speeds = self.controller.getSpeeds(0.1, 0)
19 |         self.assertAlmostEqual(speeds.left, self.ticksPerMeter*0.1)
20 |         self.assertAlmostEqual(speeds.right, self.ticksPerMeter*0.1)
21 | 
22 |     def testStraightBackward(self):
23 |         speeds = self.controller.getSpeeds(-0.1, 0)
24 |         self.assertAlmostEqual(speeds.left, -self.ticksPerMeter*0.1)
25 |         self.assertAlmostEqual(speeds.right, -self.ticksPerMeter*0.1)
26 | 
27 |     def testRotateLeft(self):
28 |         speeds = self.controller.getSpeeds(0, 1)
29 |         diffTicks = self.wheelSeparation * self.ticksPerMeter
30 |         self.assertAlmostEqual(speeds.left, -diffTicks)
31 |         self.assertAlmostEqual(speeds.right, diffTicks)
32 | 
33 |     def testRotateRight(self):
34 |         speeds = self.controller.getSpeeds(0, -1)
35 |         diffTicks = self.wheelSeparation * self.ticksPerMeter
36 |         self.assertAlmostEqual(speeds.left, diffTicks)
37 |         self.assertAlmostEqual(speeds.right, -diffTicks)
38 | 
39 |     def testCurveLeft(self):
40 |         speeds = self.controller.getSpeeds(0.1, 1)
41 |         aheadTicks = 0.1 * self.ticksPerMeter
42 |         diffTicks = self.wheelSeparation * self.ticksPerMeter
43 |         self.assertAlmostEqual(speeds.left, aheadTicks-diffTicks)
44 |         self.assertAlmostEqual(speeds.right, aheadTicks+diffTicks)
45 | 
46 |     def testMotorLimitsStraight(self):
47 |         maxTickSpeed = self.ticksPerMeter // 4
48 |         self.controller.setMaxMotorSpeed(maxTickSpeed)
49 |         speeds = self.controller.getSpeeds(1, 0)
50 |         self.assertEqual(speeds.left, maxTickSpeed)
51 |         self.assertEqual(speeds.right, maxTickSpeed)
52 | 
53 |     def testMotorLimitsCurved(self):
54 |         maxTickSpeed = self.ticksPerMeter // 4
55 |         self.controller.setMaxMotorSpeed(maxTickSpeed)
56 |         speeds = self.controller.getSpeeds(1, 1)
57 |         aheadTicks = self.ticksPerMeter
58 |         diffTicks = self.wheelSeparation * self.ticksPerMeter
59 |         factor = maxTickSpeed / (aheadTicks + diffTicks)
60 |         self.assertEqual(speeds.left,
61 |                          int((aheadTicks-diffTicks) * factor))
62 |         self.assertEqual(speeds.right, maxTickSpeed)
63 | 
64 |         
65 | if __name__ == '__main__':
66 |     unittest.main()
67 | 


--------------------------------------------------------------------------------
/src/diff_drive/odometry.py:
--------------------------------------------------------------------------------
 1 | from __future__ import division
 2 | from math import pi, sin, cos
 3 | from diff_drive.encoder import Encoder
 4 | from diff_drive.pose import Pose
 5 | 
 6 | class Odometry:
 7 |     """Keeps track of the current position and velocity of a
 8 |     robot using differential drive.
 9 |     """
10 | 
11 |     def __init__(self):
12 |         self.leftEncoder = Encoder()
13 |         self.rightEncoder = Encoder()
14 |         self.pose = Pose()
15 |         self.lastTime = 0
16 | 
17 |     def setWheelSeparation(self, separation):
18 |         self.wheelSeparation = separation
19 | 
20 |     def setTicksPerMeter(self, ticks):
21 |         self.ticksPerMeter = ticks
22 |         
23 |     def setEncoderRange(self, low, high):
24 |         self.leftEncoder.setRange(low, high)
25 |         self.rightEncoder.setRange(low, high)
26 | 
27 |     def setTime(self, newTime):
28 |         self.lastTime = newTime
29 |         
30 |     def updateLeftWheel(self, newCount):
31 |         self.leftEncoder.update(newCount)
32 | 
33 |     def updateRightWheel(self, newCount):
34 |         self.rightEncoder.update(newCount)
35 | 
36 |     def updatePose(self, newTime):
37 |         """Updates the pose based on the accumulated encoder ticks
38 |         of the two wheels. See https://chess.eecs.berkeley.edu/eecs149/documentation/differentialDrive.pdf
39 |         for details.
40 |         """
41 |         leftTravel = self.leftEncoder.getDelta() / self.ticksPerMeter
42 |         rightTravel = self.rightEncoder.getDelta() / self.ticksPerMeter
43 |         deltaTime = newTime - self.lastTime
44 | 
45 |         deltaTravel = (rightTravel + leftTravel) / 2
46 |         deltaTheta = (rightTravel - leftTravel) / self.wheelSeparation
47 | 
48 |         if rightTravel == leftTravel:
49 |             deltaX = leftTravel*cos(self.pose.theta)
50 |             deltaY = leftTravel*sin(self.pose.theta)
51 |         else:
52 |             radius = deltaTravel / deltaTheta
53 | 
54 |             # Find the instantaneous center of curvature (ICC).
55 |             iccX = self.pose.x - radius*sin(self.pose.theta)
56 |             iccY = self.pose.y + radius*cos(self.pose.theta)
57 | 
58 |             deltaX = cos(deltaTheta)*(self.pose.x - iccX) \
59 |                 - sin(deltaTheta)*(self.pose.y - iccY) \
60 |                 + iccX - self.pose.x
61 | 
62 |             deltaY = sin(deltaTheta)*(self.pose.x - iccX) \
63 |                 + cos(deltaTheta)*(self.pose.y - iccY) \
64 |                 + iccY - self.pose.y
65 | 
66 |         self.pose.x += deltaX
67 |         self.pose.y += deltaY
68 |         self.pose.theta = (self.pose.theta + deltaTheta) % (2*pi)
69 |         self.pose.xVel = deltaTravel / deltaTime if deltaTime > 0 else 0.
70 |         self.pose.yVel = 0
71 |         self.pose.thetaVel = deltaTheta / deltaTime if deltaTime > 0 else 0.
72 | 
73 |         self.lastTime = newTime
74 | 
75 |     def getPose(self):
76 |         return self.pose;
77 | 
78 |     def setPose(self, newPose):
79 |         self.pose = newPose
80 | 


--------------------------------------------------------------------------------
/nodes/diff_drive_odometry:
--------------------------------------------------------------------------------
  1 | #! /usr/bin/env python
  2 | from __future__ import division
  3 | 
  4 | import rospy
  5 | from geometry_msgs.msg import Quaternion
  6 | from geometry_msgs.msg import Twist
  7 | from nav_msgs.msg import Odometry
  8 | from std_msgs.msg import Int32
  9 | from geometry_msgs.msg import PoseWithCovarianceStamped
 10 | from tf.broadcaster import TransformBroadcaster
 11 | from tf.transformations import quaternion_from_euler, euler_from_quaternion
 12 | from math import sin, cos
 13 | from diff_drive.pose import Pose
 14 | from diff_drive import odometry
 15 | 
 16 | class OdometryNode:
 17 | 
 18 |     def __init__(self):
 19 |         self.odometry = odometry.Odometry()
 20 | 
 21 |     def main(self):
 22 |         self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
 23 |         self.tfPub = TransformBroadcaster()
 24 | 
 25 |         rospy.init_node('diff_drive_odometry')
 26 |         self.nodeName = rospy.get_name()
 27 |         rospy.loginfo("{0} started".format(self.nodeName))
 28 | 
 29 |         rospy.Subscriber("lwheel_ticks", Int32, self.leftCallback)
 30 |         rospy.Subscriber("rwheel_ticks", Int32, self.rightCallback)
 31 |         rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
 32 |                          self.on_initial_pose)
 33 | 
 34 |         self.ticksPerMeter = int(rospy.get_param('~ticks_per_meter'))
 35 |         self.wheelSeparation = float(rospy.get_param('~wheel_separation'))
 36 |         self.rate = float(rospy.get_param('~rate', 10.0))
 37 |         self.baseFrameID = rospy.get_param('~base_frame_id', 'base_link')
 38 |         self.odomFrameID = rospy.get_param('~odom_frame_id', 'odom')
 39 |         self.encoderMin = int(rospy.get_param('~encoder_min', -32768))
 40 |         self.encoderMax = int(rospy.get_param('~encoder_max', 32767))
 41 | 
 42 |         self.odometry.setWheelSeparation(self.wheelSeparation)
 43 |         self.odometry.setTicksPerMeter(self.ticksPerMeter)
 44 |         self.odometry.setEncoderRange(self.encoderMin, self.encoderMax)
 45 |         self.odometry.setTime(rospy.get_time())
 46 | 
 47 |         rate = rospy.Rate(self.rate)
 48 |         while not rospy.is_shutdown():
 49 |             self.publish()
 50 |             rate.sleep()
 51 | 
 52 |     def publish(self):
 53 |         self.odometry.updatePose(rospy.get_time())
 54 |         now = rospy.get_rostime()
 55 |         pose = self.odometry.getPose()
 56 | 
 57 |         q = quaternion_from_euler(0, 0, pose.theta)
 58 |         self.tfPub.sendTransform(
 59 |             (pose.x, pose.y, 0),
 60 |             (q[0], q[1], q[2], q[3]),
 61 |             now,
 62 |             self.baseFrameID,
 63 |             self.odomFrameID
 64 |         )
 65 | 
 66 |         odom = Odometry()
 67 |         odom.header.stamp = now
 68 |         odom.header.frame_id = self.odomFrameID
 69 |         odom.child_frame_id = self.baseFrameID
 70 |         odom.pose.pose.position.x = pose.x
 71 |         odom.pose.pose.position.y = pose.y
 72 |         odom.pose.pose.orientation.x = q[0]
 73 |         odom.pose.pose.orientation.y = q[1]
 74 |         odom.pose.pose.orientation.z = q[2]
 75 |         odom.pose.pose.orientation.w = q[3]
 76 |         odom.twist.twist.linear.x = pose.xVel
 77 |         odom.twist.twist.angular.z = pose.thetaVel
 78 |         self.odomPub.publish(odom)
 79 | 
 80 |     def on_initial_pose(self, msg):
 81 |         q = [msg.pose.pose.orientation.x,
 82 |              msg.pose.pose.orientation.x,
 83 |              msg.pose.pose.orientation.x,
 84 |              msg.pose.pose.orientation.w]
 85 |         roll, pitch, yaw = euler_from_quaternion(q)
 86 | 
 87 |         pose = Pose()
 88 |         pose.x = msg.pose.pose.position.x
 89 |         pose.y = msg.pose.pose.position.y
 90 |         pose.theta = yaw
 91 | 
 92 |         rospy.loginfo('Setting initial pose to %s', pose)
 93 |         self.odometry.setPose(pose)
 94 | 
 95 |     def leftCallback(self, msg):
 96 |         self.odometry.updateLeftWheel(msg.data)
 97 | 
 98 |     def rightCallback(self, msg):
 99 |         self.odometry.updateRightWheel(msg.data)
100 | 
101 | 
102 | if __name__ == '__main__':
103 |     try:
104 |         node = OdometryNode()
105 |         node.main()
106 |     except rospy.ROSInterruptException:
107 |         pass
108 | 


--------------------------------------------------------------------------------
/tests/test_odometry.py:
--------------------------------------------------------------------------------
  1 | #! /usr/bin/env python
  2 | from __future__ import division
  3 | PKG='test_odometry'
  4 | 
  5 | from math import pi, sin, cos
  6 | import unittest
  7 | from diff_drive.odometry import Odometry
  8 | 
  9 | 
 10 | class TestOdometry(unittest.TestCase):
 11 | 
 12 |     def setUp(self):
 13 |         self.wheelSeparation = 0.10
 14 |         self.ticksPerMeter = 10000
 15 |         self.odom = Odometry()
 16 |         self.odom.setWheelSeparation(self.wheelSeparation)
 17 |         self.odom.setTicksPerMeter(self.ticksPerMeter)
 18 | 
 19 |     def testInitialization(self):
 20 |         pose = self.odom.getPose()
 21 |         self.assertEquals(pose.x, 0)
 22 |         self.assertEquals(pose.y, 0)
 23 |         self.assertEquals(pose.theta, 0)
 24 |         self.assertEquals(pose.xVel, 0)
 25 |         self.assertEquals(pose.yVel, 0)
 26 |         self.assertEquals(pose.thetaVel, 0)
 27 | 
 28 |     def testTravelForward(self):
 29 |         self.checkUpdate(10000, 10000, 2, {'x': 1, 'xVel': 1/2})
 30 | 
 31 |     def testSpinLeft(self):
 32 |         angle = (200/self.ticksPerMeter / self.wheelSeparation) % (2*pi)
 33 |         self.checkUpdate(-100, 100, 2,
 34 |                          {'theta': angle, 'thetaVel': angle/2})
 35 | 
 36 |     def testSpinRight(self):
 37 |         angle = (200/self.ticksPerMeter / self.wheelSeparation) % (2*pi)
 38 |         self.checkUpdate(100, -100, 2,
 39 |                          {'theta': (-angle) % (2*pi),
 40 |                           'thetaVel': -angle/2})
 41 | 
 42 |     def testCurveLeft(self):
 43 |         radius = self.wheelSeparation / 2
 44 |         angle = pi
 45 |         s = angle * self.wheelSeparation
 46 |         ticks = int(s * self.ticksPerMeter)
 47 |         self.checkUpdate(0, ticks, 2,
 48 |                          {'x': 0,
 49 |                           'y': 2*radius,
 50 |                           'theta': angle,
 51 |                           'xVel': s/4,
 52 |                           'yVel': 0,
 53 |                           'thetaVel': angle/2})
 54 |                           
 55 |     def testCurveRight(self):
 56 |         radius = self.wheelSeparation / 2
 57 |         angle = pi
 58 |         s = angle * self.wheelSeparation
 59 |         ticks = int(s * self.ticksPerMeter)
 60 |         self.checkUpdate(ticks, 0, 2,
 61 |                          {'x': 0,
 62 |                           'y': -2*radius,
 63 |                           'theta': -angle,
 64 |                           'xVel': s/4,
 65 |                           'yVel': 0,
 66 |                           'thetaVel': -angle/2})
 67 |                           
 68 |     def testCircle(self):
 69 |         self.checkCircleRight(8, 9)
 70 |         self.checkCircleRight(0, 100)
 71 |         self.checkCircleRight(100, 0)
 72 | 
 73 |     def checkCircle(self, vr, vl):
 74 |         radius = abs(self.wheelSeparation/2 * (vr+vl)/(vr-vl))
 75 |         circumference = 2*pi*radius
 76 |         deltaTravel = (vr+vl)/2 * self.ticksPerMeter;
 77 |         for i in range(int(circumference/deltaTravel)):
 78 |             self.odom.updateLeftWheel(vl)
 79 |             self.odom.updateRightWheel(vr)
 80 |             self.odom.updatePose(i+1)
 81 |         self.checkPose(self.odom.getPose(), {'x': 0, 'y': 0})
 82 | 
 83 |     def checkUpdate(self, leftTicks, rightTicks, deltaTime, attrs):
 84 |         self.odom.updateLeftWheel(leftTicks)
 85 |         self.odom.updateRightWheel(rightTicks)
 86 |         self.odom.updatePose(deltaTime)
 87 |         self.checkPose(self.odom.getPose(), attrs)
 88 |         
 89 |     def checkPose(self, pose, attrs):
 90 |         for key in ['x', 'y', 'theta', 'xVel', 'yVel', 'thetaVel']:
 91 |             if key in attrs:
 92 |                 self.assertClose(
 93 |                     getattr(pose, key), attrs[key],
 94 |                             msg="{0}: {1}!={2}".format(key,
 95 |                                                     getattr(pose, key),
 96 |                                                     attrs[key]))
 97 |             else:
 98 |                 self.assertEquals(getattr(pose, key), 0, key)
 99 | 
100 |     def assertClose(self, x, y, msg):
101 |         if y == 0:
102 |             self.assertLess(abs(x), 0.0001, msg)
103 |         else:
104 |             self.assertLess(abs(x-y)/y, 0.001, msg)
105 |         
106 | if __name__ == '__main__':
107 |     unittest.main()
108 | 


--------------------------------------------------------------------------------
/config/view.rviz:
--------------------------------------------------------------------------------
  1 | Panels:
  2 |   - Class: rviz/Displays
  3 |     Help Height: 144
  4 |     Name: Displays
  5 |     Property Tree Widget:
  6 |       Expanded:
  7 |         - /Global Options1
  8 |         - /Status1
  9 |         - /Grid1
 10 |         - /Axes1
 11 |       Splitter Ratio: 0.5
 12 |     Tree Height: 1176
 13 |   - Class: rviz/Selection
 14 |     Name: Selection
 15 |   - Class: rviz/Tool Properties
 16 |     Expanded:
 17 |       - /2D Pose Estimate1
 18 |       - /2D Nav Goal1
 19 |       - /Publish Point1
 20 |     Name: Tool Properties
 21 |     Splitter Ratio: 0.588679016
 22 |   - Class: rviz/Views
 23 |     Expanded:
 24 |       - /Current View1
 25 |     Name: Views
 26 |     Splitter Ratio: 0.5
 27 |   - Class: rviz/Time
 28 |     Experimental: false
 29 |     Name: Time
 30 |     SyncMode: 0
 31 |     SyncSource: ""
 32 |   - Class: rviz_plugin_tutorials/Teleop
 33 |     Name: Teleop
 34 |     Topic: /cmd_vel
 35 | Visualization Manager:
 36 |   Class: ""
 37 |   Displays:
 38 |     - Alpha: 0.5
 39 |       Cell Size: 0.25
 40 |       Class: rviz/Grid
 41 |       Color: 160; 160; 164
 42 |       Enabled: true
 43 |       Line Style:
 44 |         Line Width: 0.0299999993
 45 |         Value: Lines
 46 |       Name: Grid
 47 |       Normal Cell Count: 0
 48 |       Offset:
 49 |         X: 0
 50 |         Y: 0
 51 |         Z: 0
 52 |       Plane: XY
 53 |       Plane Cell Count: 40
 54 |       Reference Frame: <Fixed Frame>
 55 |       Value: true
 56 |     - Class: rviz/MarkerArray
 57 |       Enabled: true
 58 |       Marker Topic: visualization_marker_array
 59 |       Name: MarkerArray
 60 |       Namespaces:
 61 |         {}
 62 |       Queue Size: 100
 63 |       Value: true
 64 |     - Class: rviz/Axes
 65 |       Enabled: true
 66 |       Length: 0.200000003
 67 |       Name: Axes
 68 |       Radius: 0.0299999993
 69 |       Reference Frame: base_link
 70 |       Value: true
 71 |   Enabled: true
 72 |   Global Options:
 73 |     Background Color: 48; 48; 48
 74 |     Default Light: true
 75 |     Fixed Frame: odom
 76 |     Frame Rate: 30
 77 |   Name: root
 78 |   Tools:
 79 |     - Class: rviz/Interact
 80 |       Hide Inactive Objects: true
 81 |     - Class: rviz/MoveCamera
 82 |     - Class: rviz/Select
 83 |     - Class: rviz/FocusCamera
 84 |     - Class: rviz/Measure
 85 |     - Class: rviz/SetInitialPose
 86 |       Topic: /initialpose
 87 |     - Class: rviz/SetGoal
 88 |       Topic: /move_base_simple/goal
 89 |     - Class: rviz/PublishPoint
 90 |       Single click: true
 91 |       Topic: /clicked_point
 92 |   Value: true
 93 |   Views:
 94 |     Current:
 95 |       Class: rviz/Orbit
 96 |       Distance: 3.00951147
 97 |       Enable Stereo Rendering:
 98 |         Stereo Eye Separation: 0.0599999987
 99 |         Stereo Focal Distance: 1
100 |         Swap Stereo Eyes: false
101 |         Value: false
102 |       Focal Point:
103 |         X: 0
104 |         Y: 0
105 |         Z: 0
106 |       Focal Shape Fixed Size: true
107 |       Focal Shape Size: 0.0500000007
108 |       Invert Z Axis: false
109 |       Name: Current View
110 |       Near Clip Distance: 0.00999999978
111 |       Pitch: 1.56979632
112 |       Target Frame: <Fixed Frame>
113 |       Value: Orbit (rviz)
114 |       Yaw: 4.71318531
115 |     Saved: ~
116 | Window Geometry:
117 |   Displays:
118 |     collapsed: false
119 |   Height: 1561
120 |   Hide Left Dock: false
121 |   Hide Right Dock: false
122 |   QMainWindow State: 000000ff00000000fd00000004000000000000019300000576fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006600fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003100000576000000e400fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000373000002340000000000000000fb0000000c00540065006c0065006f007000000004da000000cd0000005c00ffffff000000010000012200000576fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003100000576000000c200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a1000000046fc0100000002fb0000000800540069006d0065010000000000000a10000003de00fffffffb0000000800540069006d006501000000000000045000000000000000000000074b0000057600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
123 |   Selection:
124 |     collapsed: false
125 |   Teleop:
126 |     collapsed: false
127 |   Time:
128 |     collapsed: false
129 |   Tool Properties:
130 |     collapsed: false
131 |   Views:
132 |     collapsed: false
133 |   Width: 2576
134 |   X: 89
135 |   Y: 60
136 | 


--------------------------------------------------------------------------------
/src/diff_drive/goal_controller.py:
--------------------------------------------------------------------------------
  1 | from __future__ import division, print_function
  2 | from math import pi, sqrt, sin, cos, atan2
  3 | from diff_drive.pose import Pose
  4 | #import rospy
  5 | 
  6 | class GoalController:
  7 |     """Finds linear and angular velocities necessary to drive toward
  8 |     a goal pose.
  9 |     """
 10 | 
 11 |     def __init__(self):
 12 |         self.kP = 3
 13 |         self.kA = 8
 14 |         self.kB = -1.5
 15 |         self.max_linear_speed = 1E9
 16 |         self.min_linear_speed = 0
 17 |         self.max_angular_speed = 1E9
 18 |         self.min_angular_speed = 0
 19 |         self.max_linear_acceleration = 1E9
 20 |         self.max_angular_acceleration = 1E9
 21 |         self.linear_tolerance = 0.025 # 2.5cm
 22 |         self.angular_tolerance = 3/180*pi # 3 degrees
 23 |         self.forward_movement_only = False
 24 | 
 25 |     def set_constants(self, kP, kA, kB):
 26 |         self.kP = kP
 27 |         self.kA = kA
 28 |         self.kB = kB
 29 | 
 30 |     def set_max_linear_speed(self, speed):
 31 |         self.max_linear_speed = speed
 32 | 
 33 |     def set_min_linear_speed(self, speed):
 34 |         self.min_linear_speed = speed
 35 | 
 36 |     def set_max_angular_speed(self, speed):
 37 |         self.max_angular_speed = speed
 38 | 
 39 |     def set_min_angular_speed(self, speed):
 40 |         self.min_angular_speed = speed
 41 | 
 42 |     def set_max_linear_acceleration(self, accel):
 43 |         self.max_linear_acceleration = accel
 44 | 
 45 |     def set_max_angular_acceleration(self, accel):
 46 |         self.max_angular_acceleration = accel
 47 | 
 48 |     def set_linear_tolerance(self, tolerance):
 49 |         self.linear_tolerance = tolerance
 50 | 
 51 |     def set_angular_tolerance(self, tolerance):
 52 |         self.angular_tolerance = tolerance
 53 | 
 54 |     def set_forward_movement_only(self, forward_only):
 55 |         self.forward_movement_only = forward_only
 56 | 
 57 |     def get_goal_distance(self, cur, goal):
 58 |         if goal is None:
 59 |             return 0
 60 |         diffX = cur.x - goal.x
 61 |         diffY = cur.y - goal.y
 62 |         return sqrt(diffX*diffX + diffY*diffY)
 63 | 
 64 |     def at_goal(self, cur, goal):
 65 |         if goal is None:
 66 |             return True
 67 |         d = self.get_goal_distance(cur, goal)
 68 |         dTh = abs(self.normalize_pi(cur.theta - goal.theta))
 69 |         return d < self.linear_tolerance and dTh < self.angular_tolerance
 70 | 
 71 |     def get_velocity(self, cur, goal, dT):
 72 |         desired = Pose()
 73 | 
 74 |         goal_heading = atan2(goal.y - cur.y, goal.x - cur.x)
 75 |         a = -cur.theta + goal_heading
 76 | 
 77 |         # In Automomous Mobile Robots, they assume theta_G=0. So for
 78 |         # the error in heading, we have to adjust theta based on the
 79 |         # (possibly non-zero) goal theta.
 80 |         theta = self.normalize_pi(cur.theta - goal.theta)
 81 |         b = -theta - a
 82 | 
 83 |         # rospy.loginfo('cur=%f goal=%f a=%f b=%f', cur.theta, goal_heading,
 84 |         #               a, b)
 85 | 
 86 |         d = self.get_goal_distance(cur, goal)
 87 |         if self.forward_movement_only:
 88 |             direction = 1
 89 |             a = self.normalize_pi(a)
 90 |             b = self.normalize_pi(b)
 91 |         else:
 92 |             direction = self.sign(cos(a))
 93 |             a = self.normalize_half_pi(a)
 94 |             b = self.normalize_half_pi(b)
 95 | 
 96 |         # rospy.loginfo('After normalization, a=%f b=%f', a, b)
 97 | 
 98 |         if abs(d) < self.linear_tolerance:
 99 |             desired.xVel = 0
100 |             desired.thetaVel = self.kB * theta
101 |         else:
102 |             desired.xVel = self.kP * d * direction
103 |             desired.thetaVel = self.kA*a + self.kB*b
104 | 
105 |         # Adjust velocities if X velocity is too high.
106 |         if abs(desired.xVel) > self.max_linear_speed:
107 |             ratio = self.max_linear_speed / abs(desired.xVel)
108 |             desired.xVel *= ratio
109 |             desired.thetaVel *= ratio
110 | 
111 |         # Adjust velocities if turning velocity too high.
112 |         if abs(desired.thetaVel) > self.max_angular_speed:
113 |             ratio = self.max_angular_speed / abs(desired.thetaVel)
114 |             desired.xVel *= ratio
115 |             desired.thetaVel *= ratio
116 | 
117 |         # TBD: Adjust velocities if linear or angular acceleration
118 |         # too high.
119 | 
120 |         # Adjust velocities if too low, so robot does not stall.
121 |         if abs(desired.xVel) > 0 and abs(desired.xVel) < self.min_linear_speed:
122 |             ratio = self.min_linear_speed / abs(desired.xVel)
123 |             desired.xVel *= ratio
124 |             desired.thetaVel *= ratio
125 |         elif desired.xVel==0 and abs(desired.thetaVel) < self.min_angular_speed:
126 |             ratio = self.min_angular_speed / abs(desired.thetaVel)
127 |             desired.xVel *= ratio
128 |             desired.thetaVel *= ratio
129 | 
130 |         return desired
131 | 
132 |     def normalize_half_pi(self, alpha):
133 |         alpha = self.normalize_pi(alpha)
134 |         if alpha > pi/2:
135 |             return alpha - pi
136 |         elif alpha < -pi/2:
137 |             return alpha + pi
138 |         else:
139 |             return alpha
140 | 
141 |     def normalize_pi(self, alpha):
142 |         while alpha > pi:
143 |             alpha -= 2*pi
144 |         while alpha < -pi:
145 |             alpha += 2*pi
146 |         return alpha
147 | 
148 |     def sign(self, x):
149 |         if x >= 0:
150 |             return 1
151 |         else:
152 |             return -1
153 | 


--------------------------------------------------------------------------------
/nodes/diff_drive_go_to_goal:
--------------------------------------------------------------------------------
  1 | #! /usr/bin/env python
  2 | from __future__ import division
  3 | 
  4 | import rospy
  5 | from math import pi, asin, acos
  6 | from tf.transformations import euler_from_quaternion
  7 | from geometry_msgs.msg import Twist, PoseStamped
  8 | from nav_msgs.msg import Odometry
  9 | from std_msgs.msg import Float32, Bool
 10 | import actionlib
 11 | 
 12 | from diff_drive import goal_controller
 13 | from diff_drive import pose
 14 | from diff_drive.msg import GoToPoseAction, GoToPoseGoal, GoToPoseResult
 15 | 
 16 | class GoToGoalNode:
 17 | 
 18 |     def __init__(self):
 19 |         self.controller = goal_controller.GoalController()
 20 | 
 21 |     def main(self):
 22 |         rospy.init_node('diff_drive_go_to_goal')
 23 | 
 24 |         self.action_name = 'diff_drive_go_to_goal'
 25 |         self.action_server \
 26 |             = actionlib.SimpleActionServer(self.action_name, GoToPoseAction,
 27 |                                            execute_cb=self.on_execute,
 28 |                                            auto_start=False)
 29 | 
 30 |         self.action_client = actionlib.SimpleActionClient(
 31 |             'diff_drive_go_to_goal', GoToPoseAction)
 32 | 
 33 |         self.dist_pub = rospy.Publisher('~distance_to_goal',
 34 |                                         Float32, queue_size=10)
 35 |         self.twist_pub = rospy.Publisher('cmd_vel',
 36 |                                          Twist, queue_size=10)
 37 | 
 38 |         self.node_name = rospy.get_name()
 39 |         rospy.loginfo("{0} started".format(self.node_name))
 40 | 
 41 |         rospy.Subscriber('odom', Odometry, self.on_odometry)
 42 |         rospy.Subscriber('move_base_simple/goal', PoseStamped, self.on_goal)
 43 | 
 44 |         self.goal_achieved_pub = rospy.Publisher('goal_achieved', Bool,
 45 |                                                  queue_size=1)
 46 | 
 47 |         rate = rospy.get_param('~rate', 10.0)
 48 |         self.rate = rospy.Rate(rate)
 49 |         self.dT = 1 / rate
 50 | 
 51 |         self.kP = rospy.get_param('~kP', 3.0)
 52 |         self.kA = rospy.get_param('~kA', 8.0)
 53 |         self.kB = rospy.get_param('~kB', -1.5)
 54 |         self.controller.set_constants(self.kP, self.kA, self.kB)
 55 | 
 56 |         self.controller.set_linear_tolerance(
 57 |             rospy.get_param('~linear_tolerance', 0.05))
 58 |         self.controller.set_angular_tolerance(
 59 |             rospy.get_param('~angular_tolerance', 3/180*pi))
 60 | 
 61 |         self.controller.set_max_linear_speed(
 62 |             rospy.get_param('~max_linear_speed', 0.2))
 63 |         self.controller.set_min_linear_speed(
 64 |             rospy.get_param('~min_linear_speed', 0))
 65 |         self.controller.set_max_angular_speed(
 66 |             rospy.get_param('~max_angular_speed', 1.0))
 67 |         self.controller.set_min_angular_speed(
 68 |             rospy.get_param('~min_angular_speed', 0))
 69 |         self.controller.set_max_linear_acceleration(
 70 |             rospy.get_param('~max_linear_acceleration', 0.1))
 71 |         self.controller.set_max_angular_acceleration(
 72 |             rospy.get_param('~max_angular_acceleration', 0.3))
 73 | 
 74 |         # Set whether to allow movement backward. Backward movement is
 75 |         # safe if the robot can avoid obstacles while traveling in
 76 |         # reverse. We default to forward movement only since many
 77 |         # sensors are front-facing.
 78 |         self.controller.set_forward_movement_only(
 79 |             rospy.get_param('~forwardMovementOnly', True))
 80 | 
 81 |         self.init_pose()
 82 |         self.goal = None
 83 | 
 84 |         self.action_server.start()
 85 |         rospy.spin()
 86 | 
 87 |     def on_execute(self, goal):
 88 |         self.goal = self.get_angle_pose(goal.pose.pose)
 89 |         rospy.loginfo('Goal: (%f,%f,%f)', self.goal.x, self.goal.y,
 90 |                       self.goal.theta)
 91 | 
 92 |         success = True
 93 |         while not rospy.is_shutdown() and self.goal is not None:
 94 |             # Allow client to preempt the goal.
 95 |             if self.action_server.is_preempt_requested():
 96 |                 rospy.loginfo('Goal preempted')
 97 |                 self.send_velocity(0, 0)
 98 |                 self.action_server.set_preempted()
 99 |                 success = False
100 |                 break
101 |             self.publish()
102 |             self.rate.sleep()
103 |         if self.goal is None:  # Goal reached and cleared
104 |             result = GoToPoseResult()
105 |             result.success = success
106 |             self.action_server.set_succeeded(result)
107 | 
108 |     def init_pose(self):
109 |         self.pose = pose.Pose()
110 |         self.pose.x = 0
111 |         self.pose.y = 0
112 |         self.pose.theta = 0
113 | 
114 |     def publish(self):
115 |         if self.controller.at_goal(self.pose, self.goal):
116 |             desired = pose.Pose()
117 |         else:
118 |             desired = self.controller.get_velocity(self.pose, self.goal,
119 |                                                    self.dT)
120 | 
121 |         # if self.goal is not None \
122 |         #    and (desired.xVel!=0.0 or desired.thetaVel!=0.0):
123 |         #     rospy.loginfo(
124 |         #         'current=(%f,%f,%f) goal=(%f,%f,%f)  xVel=%f thetaVel=%f',
125 |         #         self.pose.x, self.pose.y, self.pose.theta,
126 |         #         self.goal.x, self.goal.y, self.goal.theta,
127 |         #         desired.xVel, desired.thetaVel)
128 | 
129 |         d = self.controller.get_goal_distance(self.pose, self.goal)
130 |         self.dist_pub.publish(d)
131 | 
132 |         self.send_velocity(desired.xVel, desired.thetaVel)
133 | 
134 |         # Forget the goal if achieved.
135 |         if self.controller.at_goal(self.pose, self.goal):
136 |             rospy.loginfo('Goal achieved')
137 |             self.goal = None
138 |             msg = Bool()
139 |             msg.data = True
140 |             self.goal_achieved_pub.publish(msg)
141 | 
142 |     def send_velocity(self, xVel, thetaVel):
143 |         twist = Twist()
144 |         twist.linear.x = xVel
145 |         twist.angular.z = thetaVel
146 |         self.twist_pub.publish(twist)
147 | 
148 |     def on_odometry(self, newPose):
149 |         self.pose = self.get_angle_pose(newPose.pose.pose)
150 | 
151 |     def on_goal(self, goal):
152 |         self.action_client.wait_for_server()
153 |         action_goal = GoToPoseGoal()
154 |         action_goal.pose.pose = goal.pose
155 |         self.action_client.send_goal(action_goal)
156 | 
157 |     def get_angle_pose(self, quaternion_pose):
158 |         q = [quaternion_pose.orientation.x,
159 |              quaternion_pose.orientation.y,
160 |              quaternion_pose.orientation.z,
161 |              quaternion_pose.orientation.w]
162 |         roll, pitch, yaw = euler_from_quaternion(q)
163 | 
164 |         angle_pose = pose.Pose()
165 |         angle_pose.x = quaternion_pose.position.x
166 |         angle_pose.y = quaternion_pose.position.y
167 |         angle_pose.theta = yaw
168 |         return angle_pose
169 | 
170 | if __name__ == '__main__':
171 |     try:
172 |         node = GoToGoalNode()
173 |         node.main()
174 |     except rospy.ROSInterruptException:
175 |         pass
176 | 


--------------------------------------------------------------------------------
/tests/test_goal_controller.py:
--------------------------------------------------------------------------------
  1 | from __future__ import division, print_function
  2 | PKG='test_goal_controller'
  3 | 
  4 | import unittest
  5 | from math import pi, sin, cos
  6 | from diff_drive.goal_controller import GoalController
  7 | from diff_drive.pose import Pose
  8 | 
  9 | 
 10 | class TestGoalController(unittest.TestCase):
 11 | 
 12 |     def setUp(self):
 13 |         self.controller = GoalController()
 14 | 
 15 |     # Test that the robot does not move when already at the goal
 16 |     # at the right heading.
 17 |     def testAtGoal(self):
 18 |         cur = Pose()
 19 |         desired = self.controller.getVelocity(cur, cur, 0.1)
 20 |         self.assertEquals(desired.xVel, 0)
 21 |         self.assertEquals(desired.thetaVel, 0)
 22 | 
 23 |     # Test that a goal pose ahead of the current position at the same
 24 |     # heading causes a straight-ahead move.
 25 |     def testStraightAhead(self):
 26 |         cur = Pose()
 27 |         goal = Pose()
 28 |         goal.x = 1
 29 |         desired = self.controller.getVelocity(cur, goal, 0.1)
 30 |         self.assertGreater(desired.xVel, 0)
 31 |         self.assertEquals(desired.thetaVel, 0)
 32 | 
 33 |     # Test that a goal pose behind the current position at the same
 34 |     # heading causes a straight-back move.
 35 |     def testStraightBack(self):
 36 |         cur = Pose()
 37 |         goal = Pose()
 38 |         goal.x = -1
 39 |         desired = self.controller.getVelocity(cur, goal, 0.1)
 40 |         self.assertLess(desired.xVel, 0)
 41 |         self.assertEquals(desired.thetaVel, 0)
 42 | 
 43 |     # Test that a goal at the current position with a leftward goal
 44 |     # heading causes a leftward rotation.
 45 |     def testRotateLeft(self):
 46 |         cur = Pose()
 47 |         goal = Pose()
 48 |         goal.theta = pi/2
 49 |         desired = self.controller.getVelocity(cur, goal, 0.1)
 50 |         self.assertEquals(desired.xVel, 0)
 51 |         self.assertGreater(desired.thetaVel, 0)
 52 | 
 53 |     # Test that a goal at the current position with a rightward goal
 54 |     # heading causes a rightward rotation.
 55 |     def testRotateRight(self):
 56 |         cur = Pose()
 57 |         goal = Pose()
 58 |         goal.theta = -pi/2
 59 |         desired = self.controller.getVelocity(cur, goal, 0.1)
 60 |         self.assertEquals(desired.xVel, 0)
 61 |         self.assertLess(desired.thetaVel, 0)
 62 | 
 63 |     # Test that a goal pose that is reachable with a forward, leftward
 64 |     # arc causes a forward movement with some leftward rotation.
 65 |     def testCurveLeft(self):
 66 |         cur = Pose()
 67 |         goal = Pose()
 68 |         goal.x = 1
 69 |         goal.y = 1
 70 |         goal.theta = pi/2
 71 |         desired = self.controller.getVelocity(cur, goal, 0.1)
 72 |         self.assertGreater(desired.xVel, 0)
 73 |         self.assertGreater(desired.thetaVel, 0)
 74 | 
 75 |     # Test that a goal pose that is reachable with a forward, rightward
 76 |     # arc causes a forward movement with some rightward rotation.
 77 |     def testCurveRight(self):
 78 |         cur = Pose()
 79 |         cur.theta = pi/2
 80 |         goal = Pose()
 81 |         goal.x = 1
 82 |         goal.y = 1
 83 |         desired = self.controller.getVelocity(cur, goal, 0.1)
 84 |         self.assertGreater(desired.xVel, 0)
 85 |         self.assertLess(desired.thetaVel, 0)
 86 | 
 87 |     # Test that a goal pose behind the robot that is reachable with a
 88 |     # leftward arc causes a backward movement with some rightward
 89 |     # rotation.
 90 |     def testCurveBackLeft(self):
 91 |         cur = Pose()
 92 |         goal = Pose()
 93 |         cur.x = 1
 94 |         cur.y = 1
 95 |         goal.theta = pi/2
 96 |         desired = self.controller.getVelocity(cur, goal, 0.1)
 97 |         self.assertLess(desired.xVel, 0)
 98 |         self.assertGreater(desired.thetaVel, 0)
 99 | 
100 |     # Test that a goal pose behind the robot that is reachable with a
101 |     # rightward arc causes a backward movement with some leftward
102 |     # rotation.
103 |     def testCurveBackRigth(self):
104 |         cur = Pose()
105 |         goal = Pose()
106 |         cur.x = 1
107 |         cur.y = 1
108 |         cur.theta = pi/2
109 |         desired = self.controller.getVelocity(cur, goal, 0.1)
110 |         self.assertLess(desired.xVel, 0)
111 |         self.assertLess(desired.thetaVel, 0)
112 | 
113 |     def testButtonHookLeft(self):
114 |         cur = Pose()
115 |         goal = Pose()
116 |         goal.x = 1
117 |         goal.y = 1
118 |         goal.theta = pi
119 |         desired = self.controller.getVelocity(cur, goal, 0.1)
120 |         self.assertGreater(desired.xVel, 0)
121 |         self.assertGreater(desired.thetaVel, 0)
122 | 
123 |     def testButtonHookRight(self):
124 |         cur = Pose()
125 |         goal = Pose()
126 |         goal.x = 1
127 |         goal.y = -1
128 |         goal.theta = -pi
129 |         desired = self.controller.getVelocity(cur, goal, 0.1)
130 |         self.assertGreater(desired.xVel, 0)
131 |         self.assertLess(desired.thetaVel, 0)
132 | 
133 |     def testGoToGoal(self):
134 |         self.checkGoToGoal(0, 0, 0, 1, 0, 0) # Straight ahead
135 |         self.checkGoToGoal(0, 0, 0, 1, 1, pi/2) # Arc left
136 |         self.checkGoToGoal(0, 0, 0, 1, 1, -pi/2) # Left, then turn right
137 |         self.checkGoToGoal(0, 0, 0, 0, 1, pi) # Go left
138 |         self.checkGoToGoal(0, 0, 0, 1, 0, pi) # Go ahead and u-turn
139 |         self.checkGoToGoal(0, 0, 0, -1, 0, 0) # Straight back
140 |         self.checkGoToGoal(0, 0, 0, -1, -1, 0) # Back up to right
141 |         self.checkGoToGoal(0, 0, 0, -1, -1, pi) # Back up and turn left
142 | 
143 |     def testGoToGoalForwardOnly(self):
144 |         self.controller.setForwardMovementOnly(True)
145 |         self.checkGoToGoal(0, 0, 0, 1, 0, 0) # Straight ahead
146 |         self.checkGoToGoal(0, 0, 0, 1, 1, pi/2) # Arc left
147 |         self.checkGoToGoal(0, 0, 0, 1, 1, -pi/2) # Left, then turn right
148 |         self.checkGoToGoal(0, 0, 0, 0, 1, pi) # Go left
149 |         self.checkGoToGoal(0, 0, 0, 1, 0, pi) # Go ahead and u-turn
150 |         self.checkGoToGoal(0, 0, 0, -1, 0, 0) # Straight back
151 |         self.checkGoToGoal(0, 0, 0, -1, -1, 0) # Back up to right
152 |         self.checkGoToGoal(0, 0, 0, -1, -1, pi) # Back up and turn left
153 | 
154 |     def checkGoToGoal(self, x0, y0, th0, x1, y1, th1):
155 |         dTol = 0.05 # 5cm
156 |         thTol = 0.04 # Approx 2.5 degrees
157 | 
158 |         self.controller.setLinearTolerance(dTol)
159 |         self.controller.setAngularTolerance(thTol)
160 | 
161 |         cur = Pose()
162 |         cur.x = x0
163 |         cur.y = y0
164 |         cur.theta = th0
165 | 
166 |         goal = Pose()
167 |         goal.x = x1
168 |         goal.y = y1
169 |         goal.theta = th1
170 | 
171 |         lastDistance = self.controller.getGoalDistance(cur, goal)
172 |         dT = 0.05
173 |         for i in range(1000):
174 |             if self.controller.atGoal(cur, goal):
175 |                 return
176 | 
177 |             desired = self.controller.getVelocity(cur, goal, dT)
178 |             newTheta = cur.theta + dT*desired.thetaVel
179 |             midTheta = (cur.theta + newTheta) / 2.0
180 |             cur.x += dT * desired.xVel * cos(midTheta)
181 |             cur.y += dT * desired.xVel * sin(midTheta)
182 |             cur.theta = newTheta
183 | 
184 |         # If we get here, we didn't reach the goal.
185 |         self.assertFalse('Did not reach the goal: p0='
186 |                          + str((x0,y0,th0))
187 |                          + ' p1=' + str((x1,y1,th1)))
188 |         
189 | if __name__ == '__main__':
190 |     unittest.main()
191 | 


--------------------------------------------------------------------------------
/README.asciidoc:
--------------------------------------------------------------------------------
  1 | = diff_drive -- Differential-Drive Controller
  2 | :imagesdir: ./images
  3 | 
  4 | This package implements ROS nodes to control and monitor a differential-drive robot.
  5 | 
  6 | The package
  7 | is intended as a lighter-weight solution than the ROS controller framework, albeit with lower
  8 | performance since it is written in Python. If you need tight, real-time control, you may want
  9 | to look at link:http://wiki.ros.org/ros_controllers[ros_controllers],
 10 | a C++ package which includes a differential-drive controller that is integrated with
 11 | link:http://wiki.ros.org/ros_control[ros_control] and
 12 | link:http://wiki.ros.org/controller_manager[controller_manager]. Those controllers are designed
 13 | to integrate with hardware in the same process, rather than using topics. Instead, this package
 14 | expects to publish the desired motor speeds using standard ROS messages.
 15 | 
 16 | == Supplied Nodes
 17 | 
 18 | * `diff_drive_controller` -- Converts from twist to wheel velocities for motors.
 19 | * `diff_drive_odometry` -- Publishes odometry from wheel encoder data.
 20 | * `diff_drive_go_to_goal` -- Moves the robot to a goal position.
 21 | * `diff_drive_mock_robot` -- Implements a mock differential drive robot, for testing.
 22 | 
 23 | The nodes in this package are designed with these considerations:
 24 | 
 25 | * The node and hardware implementing differential drive should deal only in encoder ticks.
 26 | * Conversions to and from physical coordinates should happen within the nodes in this package.
 27 | * This package should integrate cleanly with the navigation stack, perhaps with remappings.
 28 | * Nodes should use standard topic and parameter names used by the navigation stack, but should allow remapping.
 29 | 
 30 | == Demo
 31 | 
 32 | To see all the nodes in this package in action, you can launch a demo from ROS. There are no
 33 | dependencies other than the standard ROS packages.
 34 | 
 35 |      roslaunch diff_drive demo.launch
 36 |      
 37 | This launches _rviz_ as part of the demo, and shows the robot position as a small coordinate system on
 38 | 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.
 39 | Then click and drag within the grid to set the robot goal position and heading. The mock robot will move to
 40 | that new pose, which you can see by the movement of the robot axes.
 41 | 
 42 | In the demo, both forward and backward movement is allowed, so if the goal position is behind the robot,
 43 | it will move backward. You can force the robot to move foward only by setting the parameter `~forwardMovementOnly`
 44 | to `true`.
 45 | 
 46 | == ROS API
 47 | 
 48 | === 1. diff_drive_controller
 49 | 
 50 | Listens for desired linear and angular velocity, and publishes corresponding wheel velocities, in encoder ticks per second, required to achieve those velocities.
 51 | 
 52 | ==== Published Topics
 53 | 
 54 | `~lwheel_desired_rate` (std_msgs/Int32)::
 55 | Desired left wheel rotation rate, in encoder ticks per second.
 56 | 
 57 | `~rwheel_desired_rate` (std_msgs/Int32)::
 58 | Desired right wheel rotation rate, in encoder ticks per second.
 59 | 
 60 | ==== Subscribed Topics
 61 | 
 62 | `~cmd_vel` (geometry_msgs/Twist)::
 63 | Desired linear and angular velocity.
 64 | 
 65 | ==== Parameters
 66 | 
 67 | `~ticks_per_meter` (double)::
 68 | Number of encoder ticks per meter of travel.
 69 | 
 70 | `~wheel_separation` (double)::
 71 | Distance between the two wheels (meters).
 72 | 
 73 | `~rate` (int, default: 50)::
 74 | The rate that the output velocity target messages will be published (Hz).
 75 | 
 76 | `~timeout` (int, default: 0.2)::
 77 | The maximum number of seconds expected between `cmd_vel` messages. If `cmd_vel`
 78 | is not received before this limit, the controller will assume the commanding
 79 | node has died and will set the desired wheel rates to zero, to stop the robot.
 80 | 
 81 | === 2. diff_drive_odometry
 82 | 
 83 | Listens for wheel movement and rates and publishes the transform between the odom frame and the robot frame.
 84 | 
 85 | ==== Published Topics
 86 | 
 87 | `~odom` -- (nav_msgs/Odometry)::
 88 | The robot odometry -- the current robot pose.
 89 | 
 90 | `~tf`::
 91 | The transform between the odometry frame and the robot frame.
 92 | 
 93 | ==== Subscribed Topics
 94 | 
 95 | `~lwheel_ticks` (std_msgs/Int32)::
 96 | Cumulative encoder ticks of the left wheel.
 97 | 
 98 | `~rwheel_ticks` (std_msgs/Int32)::
 99 | Cumulative encoder ticks of the right wheel.
100 | 
101 | `~lwheel_rate` (std_msgs/Float32)::
102 | Left wheel rotation rate, in encoder ticks per second.
103 | 
104 | `~rwheel_rate` (std_msgs/Float32)::
105 | Right wheel rotation rate, in encoder ticks per second.
106 | 
107 | ==== Parameters
108 | 
109 | `~ticks_per_meter` (double)::
110 | Number of encoder ticks per meter of travel.
111 | 
112 | `~wheel_separation` (double)::
113 | Distance between the two wheels (m).
114 | 
115 | `~rate` (double, default 10.0)::
116 | The rate at which the `tf` and `odom` topics are published (Hz).
117 | 
118 | `~timeout` (double, default 0.2)::
119 | The amount of time to continue publishing desired wheel rates after receiving a twist message (seconds).
120 | If set to zero, wheel velocities will be sent only when a new twist message is received.
121 | 
122 | `~base_frame_id` (string, default: "base_link")::
123 | The name of the base frame of the robot. 
124 | 
125 | `~odom_frame_id` (string, default: "odom")::
126 | The name of the odometry reference frame. 
127 | 
128 | `~encoder_min` (int, default: -32768)::
129 | 
130 | `~encoder_max` (int, default: 32768)::
131 | The min and max value the encoder should output. Used to calculate odometry when the values wrap around. 
132 | 
133 | `~wheel_low_wrap` (int, default: 0.3 * (encoder_max - encoder_min + 1) + encoder_min)::
134 | 
135 | `~wheel_high_wrap` (int, default: 0.7 * (encoder_max - encoder_min + 1) + encoder_min)::
136 | 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.
137 | 
138 | === 3. diff_drive_go_to_goal
139 | 
140 | Listens for new goal poses and computes velocities needed to achieve the goal.
141 | 
142 | ==== Published Topics
143 | 
144 | `~distance_to_goal` (std_msgs/Float32)::
145 | Distance to the goal position (meters).
146 | 
147 | `~cmd_vel` (geometry_msgs/Twist)::
148 | Desired linear and angular velocity to move toward the goal pose.
149 | 
150 | ==== Subscribed Topics
151 | 
152 | `~goal` (geometry_msgs/Pose)::
153 | Desired goal pose.
154 | 
155 | ==== Parameters
156 | 
157 | `~rate` (float, default: 10)::
158 | Rate at which to publish desired velocities (Hz).
159 | 
160 | `~goal_linear_tolerance` (float, default: 0.1)::
161 | The distance from the goal at which the robot is assumed to have accomplished the goal position (meters).
162 | 
163 | `~goal_angular_tolerance` (float, default: 0.087)::
164 | The difference between robot angle and goal pose angle at which the robot is assumed to have
165 | accomplished the goal attitude (radians). Default value is approximately 5 degrees.
166 | 
167 | `~max_linear_velocity` (float, default: 0.2)::
168 | The maximum linear velocity toward the goal (meters/second).
169 | 
170 | `~max_angular_velocity` (float, default: 1.5)::
171 | The maximum angular velocity (radians/second).
172 | 
173 | `~max_linear_acceleration` (float, default: 4.0)::
174 | The maximum linear acceleration (meters/second^2).
175 | 
176 | `~forwardMovementOnly` (boolean, default: true)::
177 | If true, only forward movement is allowed to achieve the goal position.
178 | If false, the robot will move backward to the goal if that is the most
179 | direct path.
180 | 
181 | `~Kp` (float, default: 3.0)::
182 | Linear distance proportionality constant. Higher values make the robot accelerate more quickly toward the goal and decelerate less quickly.
183 | 
184 | `~Ka` (float: default: 8.0)::
185 | Proportionality constant for angle to goal position. Higher values make the robot turn more quickly toward the goal.
186 | 
187 | `~Kb` (float: default: -1.5)::
188 | 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_.
189 | 
190 | 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:
191 | 
192 |     v = Kp * d
193 |     w = Ka*a + Kb*b
194 | 
195 | See _Autonomous Mobile Robots, Second Edition_ by Siegwart et. al., section 3.6.2.4. In this code, when the robot
196 | is near enough to the goal, _v_ is set to zero, and _w_ is simply _Kb*b_.
197 | 
198 | To ensure convergence toward the goal, _K~p~_ and _K~a~_ must be positive, _K~b~_ must be negative, and _K~a~_
199 | must be greater than _K~p~_. To ensure robust convergence, so that the robot never changes direction,
200 | _K~a~_ - 5/3*_K~b~_ - 2/pi*_K~p~_ must be greater than zero.
201 | 
202 | === 4. diff_drive_mock_robot
203 | 
204 | Implements a simulation of perfect differential drive robot hardware. It immediately follows any speed
205 | commands received with infinite acceleration, and publishes the wheel encoder values and encoder
206 | rates.
207 | 
208 | ==== Published Topics
209 | 
210 | `~lwheel_ticks` (std_msgs/Int32)::
211 | Cumulative encoder ticks of the left wheel.
212 | 
213 | `~rwheel_ticks` (std_msgs/Int32)::
214 | Cumulative encoder ticks of the right wheel.
215 | 
216 | `~lwheel_rate` (std_msgs/Float32)::
217 | Left wheel rotation rate, in encoder ticks per second.
218 | 
219 | `~rwheel_rate` (std_msgs/Float32)::
220 | Right wheel rotation rate, in encoder ticks per second.
221 | 
222 | ==== Subscribed Topics
223 | 
224 | `~lwheel_desired_rate` (std_msgs/Int32)::
225 | Desired left wheel rotation rate, in encoder ticks per second.
226 | 
227 | `~rwheel_desired_rate` (std_msgs/Int32)::
228 | Desired right wheel rotation rate, in encoder ticks per second.
229 | 
230 | ==== Parameters
231 | 
232 | `~cmd_timeout` (float, default: 0.2)::
233 | The amount of time after the last wheel rate message when the robot should stop automatically (seconds).
234 | 
235 | `~rate` (float, default 10.0)::
236 | The rate at which the simulated wheel encoder values and rates should be published (Hz).
237 | 


--------------------------------------------------------------------------------