Lab A: Sample Solutions for Coding Exercises



These are sample solutions for the Python scripts from Coding Exercises #1 (Turtlebot3 Spinning in Circles) and #2 (Turtlebot3 Stopping in Front of a Wall) from Lab A.


spin_in_circles.py

#!/usr/bin/env python3

import rospy

# msgs needed for /cmd_vel
from geometry_msgs.msg import Twist, Vector3

class SpinCircles(object):
    """ This node publishes ROS messages containing the 3D coordinates of a single point """

    def __init__(self):
        # initialize the ROS node
        rospy.init_node('spin_circles')
        # setup publisher to the cmd_vel ROS topic
        self.robot_movement_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

    def run(self):
        # setup the Twist message we want to send
        my_twist = Twist(
            linear=Vector3(0.1, 0, 0),
            angular=Vector3(0, 0, 0.25)
        )

        # allow the publisher enough time to set up before publishing the first msg
        rospy.sleep(1)

        # publish the message
        self.robot_movement_pub.publish(my_twist)

if __name__ == '__main__':
    # instantiate the ROS node and run it
    node = SpinCircles()
    node.run()

stop_at_wall.py

#!/usr/bin/env python3
​
​
# TOPICS:
#   cmd_vel: publish to, used for setting robot velocity
#   scan   : subscribing, where the wall is
​
import rospy
​
# msg needed for /scan.
from sensor_msgs.msg import LaserScan
​
# msgs needed for /cmd_vel.
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Vector3
​
# How close we will get to wall.
distance = 0.4
​
class StopAtWall(object):
    """ This node walks the robot to wall and stops """
​
    def __init__(self):
        # Start rospy node.
        rospy.init_node("walk_to_wall")
​
        # Declare our node as a subscriber to the scan topic and
        #   set self.process_scan as the function to be used for callback.
        rospy.Subscriber("/scan", LaserScan, self.process_scan)
​
        # Get a publisher to the cmd_vel topic.
        self.twist_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
​
        # Create a default twist msg (all values 0).
        lin = Vector3()
        ang = Vector3()
        self.twist = Twist(linear=lin,angular=ang)
​
    def process_scan(self, data):
        # Determine closeness to wall by looking at scan data from in front of
        #   the robot, set linear velocity based on that information, and
        #   publish to cmd_vel.
​
        # This code assumes that you're using an LDS-01 LiDAR model where the
        #    first entry in data.ranges corresponds with what's directly in
        #    front of the robot. 
​
        if (data.ranges[0] == 0.0 or data.ranges[0] >= distance):
            # Go forward if not close enough to wall.
            self.twist.linear.x = 0.1
        else:
            # Close enough to wall, stop.
            self.twist.linear.x = 0
​
        # Publish msg to cmd_vel.
        self.twist_pub.publish(self.twist)
​
​
    def run(self):
        # Keep the program alive.
        rospy.spin()
​
if __name__ == '__main__':
    # Declare a node and run it.
    node = StopAtWall()
    node.run()

© Sarah Sebo