move_action_server.py

Go to the documentation of this file.

#!/usr/bin/env python
import roslib; roslib.load_manifest('move_action_server')
import rospy
import math
from geometry_msgs.msg import Twist, PoseWithCovarianceStamped
from rfnserver import RFNServer
from tf.transformations import euler_from_quaternion


robot_x = False
robot_y = False
robot_z_rot = False
pub = False



class MoveActionServer:
  server = None

  def __init__(self):
    self.server = RFNServer("move", self.cb_rfn)
    self.server.register_with_frame("moving_forward")
    self.server.register_with_frame("moving_backward")
    self.server.register_with_frame("turning_left")
    self.server.register_with_frame("turning_right")
    self.server.start()


  def cb_rfn(self, filled_semantic_frame):
    rospy.loginfo("MoveActionServer: Starting callback.")
    if filled_semantic_frame.name in ["moving_forward", "moving_backward"]:
      self.parse_and_move_forward_backward(filled_semantic_frame)
    elif filled_semantic_frame.name in ["turning_left", "turning_right"]:
      self.parse_and_turn(filled_semantic_frame)
    else:
      rospy.logerr("MoveActionServer: I didn't sign up for this!: " +
                   filled_semantic_frame.name)


  def parse_and_move_forward_backward(self, filled_semantic_frame):
    # Determine desired distance and direction
    if filled_semantic_frame.name == "moving_forward":
      direction = 1
    elif filled_semantic_frame.name == "moving_backward":
      direction = -1
    else:
      rospy.logerr("MoveActionServer: This is not moving forward/backward!: " +
                   filled_semantic_frame.name)
      self.server.set_aborted()
      return
    distance_str = RFNServer.frame_argument(filled_semantic_frame,
                                            "distance")
    distance_unit_str = RFNServer.frame_argument(filled_semantic_frame,
                                                 "distance_unit")

    # Convert distance units to meters
    if distance_unit_str in ["meter", "meters"]:
      distance_scale = 1
    elif distance_unit_str in ["millimeter", "millimeters"]:
      distance_scale = 0.001
    elif distance_unit_str in ["centimeter", "centimeters"]:
      distance_scale = 0.01
    elif distance_unit_str in ["kilometer", "kilometers"]:
      distance_scale = 1000
    elif distance_unit_str in ["inch", "inches"]:
      distance_scale = 0.0254
    elif distance_unit_str in ["foot", "feet"]:
      distance_scale = 0.3048
    elif distance_unit_str in ["yard", "yards"]:
      distance_scale = 0.9144
    elif distance_unit_str in ["furlong", "furlongs"]:
      distance_scale = 201.16800
    elif distance_unit_str in ["mile", "miles"]:
      distance_scale = 1609.344
    else:
      rospy.logerr("parse_and_move_forward_backward: Unrecognized distance " +
                   " unit: " + distance_unit_str)
      self.server.set_aborted()
      return

    distance = float(distance_str) * distance_scale
    self.move_forward_backward(direction * distance)



  def parse_and_turn(self, filled_semantic_frame):
    # Determine desired distance and direction
    if filled_semantic_frame.name == "turning_left":
      direction = 1 # (like in math/physics -- right hand rule)
    elif filled_semantic_frame.name == "turning_right":
      direction = -1
    else:
      rospy.logerr("MoveActionServer: This is not turning left/right!: " +
                   filled_semantic_frame.name)
      self.server.set_aborted()
      return
    distance_str = RFNServer.frame_argument(filled_semantic_frame,
                                            "distance")
    distance_unit_str = RFNServer.frame_argument(filled_semantic_frame,
                                                 "distance_unit")

    # Convert distance units to meters
    if distance_unit_str in ["radian", "radians"]:
      distance_scale = 1
    elif distance_unit_str in ["degree", "degrees"]:
      distance_scale = 0.0174532925
    else:
      rospy.logerr("parse_and_turn: Unrecognized distance unit: " +
                   distance_unit_str)
      self.server.set_aborted()
      return

    distance = float(distance_str) * distance_scale
    self.turn(direction * distance)



  def move_forward_backward(self, distance):
    global robot_x, robot_y
    """ Move distance meters.  (positive == forwards; negative == backwards)"""
    rospy.loginfo("Moving " + str(distance) + " meters.")

    # Get initial position
    start_x = robot_x
    start_y = robot_y

    # Wait until we've moved desired distance
    r = rospy.Rate(25) # hz
    while (not rospy.is_shutdown() and
           not self.server.is_preempt_requested() and
           (math.sqrt((start_x - robot_x)**2 + (start_y - robot_y)**2) <
            abs(distance))):
      # Tell robot to start moving
      t = Twist()
      t.linear.x = math.copysign(0.5, distance) # m/s
      t.linear.y = 0
      t.linear.z = 0
      t.angular.x = 0
      t.angular.y = 0
      t.angular.z = 0
      pub.publish(t)
      r.sleep()

    # Tell robot to stop moving forward
    t = Twist()
    t.linear.x = 0
    t.linear.y = 0
    t.linear.z = 0
    t.angular.x = 0
    t.angular.y = 0
    t.angular.z = 0
    pub.publish(t)

    if self.server.is_preempt_requested():
      self.server.set_preempted()
    else:
      self.server.set_succeeded()



  def turn(self, distance):
    global robot_z_rot
    """ Move distance radians.

    (positive == counterclockwise; negative == clockwise)
    """
    rospy.loginfo("Moving " + str(distance) + " radians.")

    # Get initial position.  Angle is in [-pi, pi]
    start_z = robot_z_rot
    end_z = (start_z + distance + math.pi) % (2 * math.pi) - math.pi


    # WARNING: The below calculation is numerically unstable
    #   near the poles!
    # Wait until we've moved desired distance
    r = rospy.Rate(25) # hz
    while (not rospy.is_shutdown() and
           not self.server.is_preempt_requested() and
           not (abs(robot_z_rot - end_z) < 0.15)):
      rospy.loginfo("robot_z_rot " + str(robot_z_rot) +
                    " end_z " + str(end_z) +
                    " absdiff " + str(abs(robot_z_rot - end_z)))
      # Tell robot to start moving
      t = Twist()
      t.linear.x = 0
      t.linear.y = 0
      t.linear.z = 0
      t.angular.x = 0
      t.angular.y = 0
      t.angular.z = math.copysign(1.5, distance) # rad/s
      pub.publish(t)
      r.sleep()

    # Tell robot to stop moving forward
    t = Twist()
    t.linear.x = 0
    t.linear.y = 0
    t.linear.z = 0
    t.angular.x = 0
    t.angular.y = 0
    t.angular.z = 0
    pub.publish(t)

    if self.server.is_preempt_requested():
      self.server.set_preempted()
    else:
      self.server.set_succeeded()



def cb_odom(msg):
  global robot_x, robot_y, robot_z_rot
  robot_x = msg.pose.pose.position.x
  robot_y = msg.pose.pose.position.y
  q = msg.pose.pose.orientation
  robot_z_rot = euler_from_quaternion([q.x, q.y, q.z, q.w])[2]
  # Rotation is between -pi and pi (in radians)



def main():
  global pub
  rospy.init_node('move_action_server')
  pub = rospy.Publisher('turtlebot_node/cmd_vel', Twist)
  rospy.Subscriber('robot_pose_ekf/odom_combined', PoseWithCovarianceStamped, cb_odom)
  rospy.logwarn("move_action_server was implemented for a demo, not as a " +
                "serious navigation engine.  So, be careful using this, " +
                "especially the turn commands, with any sort of expensive " +
                "robot.  Otherwise, enjoy!")
  server = MoveActionServer()
  rospy.spin()



if __name__ == '__main__':
  main()