turtlebot_control.py

#!/usr/bin/env python
#The line above tells Linux that this file is a Python script,
#and that the OS should use the Python interpreter in /usr/bin/env
#to run it. Don't forget to use "chmod +x [filename]" to make
#this script executable.

#Import the rospy package. For an import to work, it must be specified
#in both the package manifest AND the Python file in which it is used.
import rospy
import tf2_ros
import sys
from std_msgs.msg import String

from geometry_msgs.msg import Twist

#Define the method which contains the main functionality of the node.
def controller():
  """
  Controls a turtlebot whose position is denoted by turtlebot_frame,
  to go to a position denoted by target_frame
  Inputs:
  - turtlebot_frame: the tf frame of the AR tag on your turtlebot
  - target_frame: the tf frame of the target AR tag
  """

  ################################### YOUR CODE HERE ##############

  #Create a publisher and a tf buffer, which is primed with a tf listener
  pub = rospy.Publisher('mobile_base/commands/velocity', Twist, queue_size=10)
  pub7 = rospy.Publisher('notify_7bot', String, queue_size=10)
  tfBuffer = tf2_ros.Buffer()
  tfListener = tf2_ros.TransformListener(tfBuffer)

  found_copy_blue = False
  found_copy_red = False
  
  # Create a timer object that will sleep long enough to result in
  # a 10Hz publishing rate
  r = rospy.Rate(10) # 10hz

  K1 = 0.3
  K2 = 1
  # Loop until the node is killed with Ctrl-C
  while not rospy.is_shutdown():
    try:
      msg = ""
      trans = None
      # Find red first
      if not found_copy_red:
        trans = tfBuffer.lookup_transform("base_link", "copy_frame_red", rospy.Time())
        msg = "red"
      elif not found_copy_blue:
        trans = tfBuffer.lookup_transform("base_link", "copy_frame_blue", rospy.Time())
        msg = "blue"
      # Process trans to get your state error
      # Generate a control command to send to the robot

      print(trans, msg)
      
      d2 = trans.transform.translation.x ** 2 + trans.transform.translation.y ** 2
      print(d2)
      if d2 < .05:
        if msg is "red":
          pub7.publish("red")
        elif msg is "blue":
          pub7.publish("blue")
        print("brrrrrrr")
        if not found_copy_red:
          found_copy_red = True
          continue
        elif not found_copy_blue:
          found_copy_blue = True
          break


      x = K1 * (trans.transform.translation.x)
      theta = K2 * (trans.transform.translation.y)
      t = Twist()
      t.linear.x = x
      t.angular.z = theta
      control_command = t # Generate this

      #################################### end your code ###############

      pub.publish(control_command)
    except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
      pass
    # Use our rate object to sleep until it is time to publish again
    r.sleep()

      
# This is Python's sytax for a main() method, which is run by default
# when exectued in the shell
if __name__ == '__main__':
  # Check if the node has received a signal to shut down
  # If not, run the talker method

  #Run this program as a new node in the ROS computation graph 
  #called /turtlebot_controller.
  rospy.init_node('turtlebot_controller', anonymous=True)

  try:
    controller()
  except rospy.ROSInterruptException:
    pass