Go to the documentation of this file.00001 '''
00002 basic_move_controller.py
00003 LICENSE : BSD - https://raw.github.com/yujinrobot/yujin_ocs/license/LICENSE
00004 '''
00005 import math
00006 import copy
00007 import rospy
00008 import tf
00009 import nav_msgs.msg as nav_msgs
00010 import geometry_msgs.msg as geometry_msgs
00011
00012 class BasicMoveController(object):
00013 def __init__(self, odom_topic='odom', cmd_vel_topic='cmd_vel'):
00014 self._sub_odom = rospy.Subscriber(odom_topic, nav_msgs.Odometry, self.process_odometry)
00015 self._pub_cmd_vel = rospy.Publisher(cmd_vel_topic, geometry_msgs.Twist, queue_size=5)
00016
00017 self._odom = None
00018
00019 def process_odometry(self, msg):
00020 self._odom = copy.deepcopy(msg)
00021
00022 def move_at(self, v, w, t):
00023 vel = geometry_msgs.Twist()
00024
00025 vel.linear.x = v
00026 vel.angular.z = w
00027 self._pub_cmd_vel.publish(vel)
00028 rospy.sleep(t)
00029
00030 def turn_clockwise(self):
00031 self.move_at(0.0, -0.5, 0.1)
00032
00033 def turn_counter_clockwise(self):
00034 self.move_at(0.0, 0.5, 0.1)
00035
00036 def turn(self, angle):
00037 raise NotImplementedError()
00038
00039 def spin_clockwise(self):
00040 yaw = self._get_odom_yaw()
00041
00042 i = 0
00043 while (self._get_odom_yaw() <= yaw) or i < 5:
00044 self.turn_clockwise()
00045 i = i + 1
00046
00047 i = 0
00048 while (self._get_odom_yaw() > yaw) or i < 5:
00049 self.turn_clockwise()
00050 i = i +1
00051
00052 def _get_odom_yaw(self):
00053 quaternion = (self._odom.pose.pose.orientation.x, self._odom.pose.pose.orientation.y, self._odom.pose.pose.orientation.z, self._odom.pose.pose.orientation.w)
00054 roll, pitch, yaw = tf.transformations.euler_from_quaternion(quaternion)
00055 return yaw
00056
00057 def slow_backward(self):
00058 self.move_at(-0.1, 0.0, 0.1)
00059
00060 def backward(self, distance):
00061 pos0 = copy.deepcopy(self._odom.pose.pose.position)
00062
00063 while(self._distance2d(pos0, self._odom.pose.pose.position) < distance):
00064 self.slow_backward()
00065
00066 def _distance2d(self, p1, p2):
00067 '''
00068 computes distance between two points
00069 '''
00070 return math.sqrt(math.pow((p2.x - p1.x), 2) + math.pow((p2.y -p1.y), 2))