initial commit

LICENSE

@@ -1,3 +1,4 @@
+
                                  Apache License
                            Version 2.0, January 2004
                         http://www.apache.org/licenses/

package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>scripted_bot_driver</name>
+  <version>0.0.0</version>
+  <description>Scripts for driving a generic bot from a motion-file</description>
+  <maintainer email="[email protected]">Paul Bouchier</maintainer>
+  <license>Apache-2.0</license>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

scripted_bot_driver/drive_straight_odom.py

@@ -0,0 +1,91 @@
+#!/usr/bin/env python
+
+import sys
+import time
+import rospy
+from geometry_msgs.msg import Twist
+from mowbot_msgs.msg import OdomExtra, PlatformData
+import tf
+from math import radians, copysign, sqrt, pow, pi, asin, atan2
+from MoveParent import MoveParent
+
+loop_rate = 10       # loop rate
+
+class DriveStraightOdom(MoveParent):
+    def __init__(self, cmd_vel):
+        super().__init__(cmd_vel)
+
+    def parse_argv(self, argv):
+        self.distance = float(argv[0])  # pick off first arg from supplied list
+        self.odometer_goal = self.odom_extra.odometer + self.distance
+        self.odometer_start = self.odom_extra.odometer
+
+        # check whether a speed argument was supplied
+        if len(argv) > 1:
+            try:
+                speed_arg = float(argv[1])
+                self.speed = speed_arg
+                print('Using supplied speed {}'.format(self.speed))
+                return 2
+            except ValueError:
+                return 1
+        return 1            # return number of args consumed
+
+    def print(self):
+        rospy.loginfo('Drive straight with odometry for {} m'.format(self.distance))
+
+    # run is called at the rate until it returns true
+    def run(self):
+        if self.once:
+            rospy.loginfo('start odometer: {}, goal: {}'.format(self.odom_extra.odometer, self.odometer_goal))
+            self.once = False
+
+        if ((self.distance >= 0 and self.odom_extra.odometer >= self.odometer_goal) or \
+            (self.distance < 0 and self.odom_extra.odometer < self.odometer_goal)):
+            rospy.loginfo('traveled: {} m'.format(self.odom_extra.odometer - self.odometer_start))
+            return True
+
+        # accelerate to full speed as long as we haven't reached the goal
+        if self.distance >= 0:
+            self.move_cmd.linear.x = self.slew_vel(self.speed)
+        else:
+            self.move_cmd.linear.x = self.slew_vel(-self.speed)
+
+        rospy.loginfo(self.move_cmd.linear.x)
+        self.cmd_vel.publish(self.move_cmd)
+        return False
+
+def shutdown():
+    # Always stop the robot when shutting down the node.
+    rospy.loginfo("Stopping the robot...")
+    cmd_vel.publish(Twist())
+    rospy.sleep(1)
+
+def usage():
+    print('Usage: drive_straight.py <distance> [speed] - drive the specified distance forward or backward, with optional speed')
+    sys.exit()
+
+if __name__ == '__main__':
+    if len(sys.argv) != 2 and len(sys.argv) != 3:
+        usage()
+    argv_index = 1
+
+    rospy.init_node('move', anonymous=False)
+    rospy.on_shutdown(shutdown)     # Set rospy to execute a shutdown function when exiting
+
+    global cmd_vel
+    cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
+    r = rospy.Rate(loop_rate)
+
+    try:
+        m = DriveStraightOdom(cmd_vel)
+        argv_index += m.parse_argv(sys.argv[argv_index:])
+        m.print()
+        while (not rospy.is_shutdown()):
+            if m.run():
+                break
+            r.sleep()
+
+    except Exception as e:
+        print(e)
+        rospy.loginfo("{} node terminated.".format(__file__))

scripted_bot_driver/move_parent.py

@@ -0,0 +1,100 @@
+#!/usr/bin/env python
+
+'''
+This is the parent class of several move child classes.
+It provides support commont to all the move classes
+'''
+import sys
+import time
+import threading
+
+import rclpy
+from rclpy.node import Node
+
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+
+
+loop_rate = 10       # loop rate
+speed_default = 0.35    # driving speed, fwd or back
+low_speed_default = 0.15
+vel_slew_rate = 0.25 / loop_rate  # m/s^2 per loop
+rot_speed_default = 0.25    # rotating speed, rad/s
+low_rot_speed_default = 0.25
+rot_slew_rate = 0.5 / loop_rate  # rad/s^2
+
+class MoveParent(Node):
+
+    def __init__(self, node_name):
+        super().__init__(node_name)
+        self.once = True
+        self.speed = speed_default
+        self.full_speed = speed_default
+        self.low_speed = low_speed_default
+        self.rot_speed = rot_speed_default
+        self.full_rot_speed = rot_speed_default
+        self.low_rot_speed = low_rot_speed_default
+        self.commandedLinear = 0
+        self.commandedAngular = 0
+
+        # subscribers to robot data
+        self.odom = Odometry()
+        self.subscription = self.create_subscription(
+            Odometry, 'odom', self.odom_callback, 10)
+
+        # Publisher to control robot motion
+        self.move_cmd = Twist()
+        self.cmd_vel_pub = self.create_publisher(
+            Twist, 'cmd_vel', 10
+        )
+
+    def send_move_cmd(self, linear, angular):
+        self.move_cmd.linear.x = linear
+        self.move_cmd.angular.z = angular
+        self.commandedLinear = linear
+        self.commandedAngular = angular
+        self.cmd_vel_pub.publish(self.move_cmd)
+
+    def slew_vel(self, to):
+        return self.slew(self.commandedLinear, to, vel_slew_rate)
+
+    def slew_rot(self, to):
+        return self.slew(self.commandedAngular, to, rot_slew_rate)
+
+    def slew(self, current, to, slew_rate):
+        diff = to - current
+        if diff > slew_rate:
+            return current + slew_rate
+        if diff < -slew_rate:
+            return current - slew_rate
+        return to
+
+    def odom_callback(self, odom_msg):
+        self.odom = odom_msg
+
+def main(args=None):
+    rclpy.init()
+
+    move_parent = MoveParent('move_parent')
+
+    # Run spin in a thread, make thread daemon so we don't have to join it to exit
+    # Provide stop_node so it calls rclpy.spin(stop_node)
+    thread = threading.Thread(target=rclpy.spin, args=(move_parent, ), daemon=True)
+    thread.start()
+
+    move_parent.send_move_cmd(1.0, 0.0)
+    print('sent move command')
+    time.sleep(1)
+    move_parent.send_move_cmd(0.0, 0.0)
+    print('sent stop command')
+    time.sleep(1)
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    move_parent.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

scripted_bot_driver/stop.py

@@ -0,0 +1,105 @@
+#!/usr/bin/env python
+
+import sys
+import threading
+import time
+
+import rclpy
+from rclpy.node import Node
+from rclpy.duration import Duration
+
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+
+from scripted_bot_driver.move_parent import MoveParent
+
+loop_rate = 10       # loop rate
+
+class Stop(MoveParent):
+    def __init__(self):
+        super().__init__('stop')
+        self.pause = 0            # pause after stop, in seconds
+        self.end_pause_time = self.get_clock().now() # time when pause ends
+
+    def parse_argv(self, argv):
+        # check whether a pause argument was supplied
+        if (len(argv) > 0):
+            try:
+                pause_arg = float(argv[0])
+                self.pause = pause_arg
+                self.end_pause_time = self.get_clock.now() + Duration(self.pause)  # time when pause ends
+                self.get_logger().info('Stop is using supplied pause {}'.format(self.pause))
+                return 1
+            except ValueError:
+                return 0
+        return 0            # return number of args consumed
+
+    def print(self):
+        print('Stop and pause for {} seconds'.format(self.pause))
+
+    def run(self):
+        if self.once:
+            self.get_logger().info('Stopping from speed: {}, rot_speed: {}'.format(
+                self.odom.twist.twist.linear.x,
+                self.odom.twist.twist.angular.z))
+            self.once = False
+
+        # loop sending stop commands until both linear & angular request stopped
+        if (abs(self.commandedLinear) < 0.01 and
+            abs(self.commandedAngular) < 0.01):
+            self.send_move_cmd(0, 0)   # stop the robot
+            if self.get_clock().now() > self.end_pause_time:
+                self.get_logger.info('Stop paused for {} seconds'.format(self.pause))
+                return True
+            else:
+                return False
+
+        self.send_move_cmd(self.slew_vel(0), self.slew_rot(0))
+        return False
+
+    def on_shutdown(self):
+        # Always stop the robot when shutting down the node.
+        self.get_logger().info("Stopping the robot...")
+        self.send_move_cmd(0.0, 0.0)
+        time.sleep(1)
+
+def usage():
+    print('Usage: stop.py [pause] - ramp down linear & rotational speed & pause if pause is not zero')
+    sys.exit()
+
+def main():
+    if len(sys.argv) != 1:
+        usage()
+    argv_index = 1
+
+    rclpy.init()
+    stop_node = Stop()
+
+    # Run spin in a thread, make thread daemon so we don't have to join it to exit
+    # Provide stop_node so it calls rclpy.spin(stop_node)
+    thread = threading.Thread(target=rclpy.spin, args=(stop_node, ), daemon=True)
+    thread.start()
+
+    r = stop_node.create_rate(loop_rate)
+
+    try:
+        argv_index += stop_node.parse_argv(sys.argv[argv_index:])
+        stop_node.print()
+        while (rclpy.ok()):
+            if stop_node.run():
+                break
+            r.sleep()
+
+    except Exception as e:
+        print(e)
+        stop_node.get_logger().info("{} node terminated.".format(__file__))
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    stop_node.on_shutdown()     # stop_node should make things safe
+    stop_node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/scripted_bot_driver
+[install]
+install_scripts=$base/lib/scripted_bot_driver

setup.py

@@ -0,0 +1,28 @@
+from setuptools import find_packages, setup
+
+package_name = 'scripted_bot_driver'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='Paul Bouchier',
+    maintainer_email='[email protected]',
+    description='Scripts for driving a generic bot from a motion-file',
+    license='Apache-2.0',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'runner.py = scripted_bot_driver.runner:main',
+            'stop.py = scripted_bot_driver.stop:main',
+            'move_parent.py = scripted_bot_driver.move_parent:main',
+        ],
+    },
+)

test/test_copyright.py

@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'