教程链接

原理：移动机器人数学模型的逆运动学
算法：
1.调整机器人的朝向，直到机器人朝向与初始位置到目标位置连线方向一致
2.沿着当前方向直行，直至目标位置
3，调整机器人的朝向，直到与目标方向一致。

import rospy
from geometry_msgs.msg import Twist, Point, Quaternion
import tf
from math import radians, copysign, sqrt, pow, pi, atan2
from tf.transformations import euler_from_quaternion
import numpy as npmsg = """
control your Turtlebot3!
-----------------------
Insert xyz - coordinate.
x : position x (m)
y : position y (m)
z : orientation z (degree: -180 ~ 180)
If you want to close, insert 's'
-----------------------
"""class GotoPoint():def __init__(self):rospy.init_node('turtlebot3_pointop_key', anonymous=False)rospy.on_shutdown(self.shutdown)                               #在节点关闭时调用shutdown函数self.cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=5)  #发布者 cmd_vel话题 position = Point()                   #实例化Point类 move_cmd = Twist()                   #实例化Twist类r = rospy.Rate(10)                #设定一个频率，然后通过睡眠度过一个循环中剩下的时间   self.tf_listener = tf.TransformListener()  #接收tf变换数据self.odom_frame = 'odom'try:self.tf_listener.waitForTransform(self.odom_frame, 'base_footprint', rospy.Time(), rospy.Duration(1.0))self.base_frame = 'base_footprint'except (tf.Exception, tf.ConnectivityException, tf.LookupException):try:self.tf_listener.waitForTransform(self.odom_frame, 'base_link', rospy.Time(), rospy.Duration(1.0))self.base_frame = 'base_link'except (tf.Exception, tf.ConnectivityException, tf.LookupException):rospy.loginfo("Cannot find transform between odom and base_link or base_footprint")rospy.signal_shutdown("tf Exception")(position, rotation) = self.get_odom()last_rotation = 0linear_speed = 1angular_speed = 1(goal_x, goal_y, goal_z) = self.getkey()       #得到终端输入的数据if goal_z > 180 or goal_z < -180:print("you input wrong z range.")self.shutdown()goal_z = np.deg2rad(goal_z)         #度转换为弧度#平方差公式，求目标点和当前位置的距离，sqrt 平方根  pow幂函数double pow(double x, double y) 返回 x 的 y 次幂，即 xy。goal_distance = sqrt(pow(goal_x - position.x, 2) + pow(goal_y - position.y, 2)) distance = goal_distancewhile distance > 0.05:(position, rotation) = self.get_odom()x_start = position.xy_start = position.ypath_angle = atan2(goal_y - y_start, goal_x- x_start)  #反正切if path_angle < -pi/4 or path_angle > pi/4:if goal_y < 0 and y_start < goal_y:path_angle = -2*pi + path_angleelif goal_y >= 0 and y_start > goal_y:path_angle = 2*pi + path_angleif last_rotation > pi-0.1 and rotation <= 0:rotation = 2*pi + rotationelif last_rotation < -pi+0.1 and rotation > 0:rotation = -2*pi + rotationmove_cmd.angular.z = angular_speed * path_angle-rotationdistance = sqrt(pow((goal_x - x_start), 2) + pow((goal_y - y_start), 2))move_cmd.linear.x = min(linear_speed * distance, 0.1)if move_cmd.angular.z > 0:move_cmd.angular.z = min(move_cmd.angular.z, 1.5)else:move_cmd.angular.z = max(move_cmd.angular.z, -1.5)last_rotation = rotationself.cmd_vel.publish(move_cmd)r.sleep()(position, rotation) = self.get_odom()while abs(rotation - goal_z) > 0.05: #abs()绝对值   到达目标点后，通过原地旋转调整小车的姿态(position, rotation) = self.get_odom()if goal_z >= 0:if rotation <= goal_z and rotation >= goal_z - pi:move_cmd.linear.x = 0.00move_cmd.angular.z = 0.5else:move_cmd.linear.x = 0.00move_cmd.angular.z = -0.5else:if rotation <= goal_z + pi and rotation > goal_z:move_cmd.linear.x = 0.00move_cmd.angular.z = -0.5else:move_cmd.linear.x = 0.00move_cmd.angular.z = 0.5self.cmd_vel.publish(move_cmd)r.sleep()rospy.loginfo("Stopping the robot...")self.cmd_vel.publish(Twist())def getkey(self):          #从终端中获取 x y z 并转为float类型x, y, z = raw_input("| x | y | z |\n").split()  #通过指定分隔符对字符串进行切片if x == 's':self.shutdown()x, y, z = [float(x), float(y), float(z)]return x, y, zdef get_odom(self):try:#lookupTransform  得两个坐标系之间转换的关系，包括旋转与平移。 (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))  rotation = euler_from_quaternion(rot)   #四元数与欧拉角的转换except (tf.Exception, tf.ConnectivityException, tf.LookupException):rospy.loginfo("TF Exception")returnreturn (Point(*trans), rotation[2])def shutdown(self):                    #节点关闭时的回调函数，发布速度数据self.cmd_vel.publish(Twist())rospy.sleep(1)if __name__ == '__main__':try:while not rospy.is_shutdown():print(msg)GotoPoint()except:rospy.loginfo("shutdown program.")