Tutorials:Safety Controller

From RacecarWiki
Jump to: navigation, search
The Racecar mux system
The Racecar mux system
#!/usr/bin/env python

import rospy
from ackermann_msgs.msg import AckermannDriveStamped

class SafetyController:

  def __init__(self):
    DRIVE_TOPIC = rospy.get_param("racecar_seminar/drive_topic")
    MOVER_RATE = rospy.get_param("racecar_seminar/lab1_mover/mover_rate")

    # Initialize a timer
    self.timer = rospy.Timer(
        rospy.Duration(1./MOVER_RATE), 
        self.timerCallback)

    # Publish to the drive topic
    self.commandPub = rospy.Publisher(
        DRIVE_TOPIC,
        AckermannDriveStamped,
        queue_size=1)

    # Subscribe to the scan topic
    self.scanSub = rospy.Subscriber(
        SCAN_TOPIC,
        LaserScan,
        self.scanCallback,
        queue_size=1)

  def scanCallback(self, msg):
    ##################################################
    # TODO >>>
    # Make the robot move!
    # Create a drive message and populate it with
    # a steering angle and a velocity, then publish it
    # to the publisher, self.commandPub.
    #
    # You can find documentation for the drive message
    # here:
    # 
    #     rosmsg show AckermannDriveStamped
    #
    # As an additional challenge, can you use this
    # function to have the robot do something more
    # interesting? Like zig-zagging back and forth,
    # tracing out a figure 8?
    ##################################################
    
    # Construct the drive command
    command = AckermannDriveStamped()
    command.drive.steering_angle = 0.2
    command.drive.speed = 1.

    # Publish the drive command
    self.commandPub.publish(command)

    ##################################################
    # <<< TODO
    ##################################################

if __name__ == "__main__":
  rospy.init_node("SafetyController")
  mover = SafetyController()
  rospy.spin()