ROS_Python编程 之 案例代码核心解析（第四版）
通过Handsfree mini机器人平台配套的中级教程，我对ros_python 进阶编程 的知识做以下归纳：
文章目录
5. 进阶编程
handsfree mini 机器人工控机自带的ros系统一共有三份代码，分别是 get_angular_odom.py，nav_obstacle.py 和 nav_square.py
- get_angular_odom.py 获取偏航角度（角度制）
- nav_obstacle.py 一直沿x轴方向运动，直到感知到附近有障碍物时停下
- nav_square.py 使用直接发速度的方式画出一个正方形
5.1 get_angular_odom.py
get_angular_odom.py 的源码如下：
#! /usr/bin/python
## coding:utf-8
import PyKDL
import rospy
from sensor_msgs.msg import Imu
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
from math import *
import threading
import os
import subprocess
import yaml
#四元数转欧拉角
def quat_to_angle(quat):
rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
return rot.GetRPY()[2]
def normalize_angle(angle):
res = angle
while res > pi:
res -= 2.0*pi
while res < -pi:
res += 2.0*pi
return res
class CalibrateRobot1:
def __init__(self):
self.lock = threading.Lock()#创建锁
self.odom_frame = rospy.get_param('~odom_frame', '/mobile_base/mobile_base_controller/odom')
self.sub_imu = rospy.Subscriber(self.odom_frame, Odometry, self.imu_cb)
self.last_imu_angle = 0
self.imu_angle = 0
while not rospy.is_shutdown():
rospy.sleep(0.3)
rospy.loginfo("imu_angle:"+str((self.imu_angle)*180/pi))
def imu_cb(self, msg):
with self.lock:#自动进行上锁，等语句运行完毕，自动解锁
angle = quat_to_angle(msg.pose.pose.orientation)
self.imu_angle = angle
self.imu_time = msg.header.stamp
def main():
rospy.init_node('scan_to_angle1')
CalibrateRobot1()
if __name__ == '__main__':
main()
sensor_msgs/Imu Message 的定义如下：
Header header
geometry_msgs/Quaternion orientation
float64[9] orientation_covariance #朝向的协方差矩阵
geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance #角速度的协方差矩阵
geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance #线加速度的协方差矩阵
nav_msgs/Odometry Message 的定义如下：
Header header
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/TwistWithCovariance twist
geometry_msgs/Twist Message 的定义如下：
Vector3 linear
Vector3 angular
5.2 nav_obstacle.py
nav_obstacle.py 的源码如下：
#!/usr/bin/env python
# coding:utf-8
import rospy
import math
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
class Obstacle():
def __init__(self):
self.velocity_topic = rospy.get_param('~vel_topic', '/mobile_base/mobile_base_controller/cmd_vel')
self.speed_linear = rospy.get_param('~speed_linear', 0.3)
self.stop_distance = rospy.get_param('~stop_distance', 0.5)
self.rate_pub = rospy.get_param('~velocity_pub_rate', 10)
self.rate = rospy.Rate(self.rate_pub)
self._cmd_pub = rospy.Publisher(self.velocity_topic, Twist, queue_size=1)
self.obstacle()
def get_scan(self):#接收scan topic并将其信息储存起来
scan = rospy.wait_for_message('scan', LaserScan)
scan_filter = []
samples = len(scan.ranges)
if samples == 0:
rospy.logerr('can`t find the scan')
else:
scan_filter.extend(scan.ranges)
for i in range(samples):
if scan_filter[i] == float('Inf') or math.isnan(scan_filter[i]):
scan_filter[i] = 10
return scan_filter
def obstacle(self):
twist = Twist()
turtlebot_moving = True
while not rospy.is_shutdown():
lidar_distances = self.get_scan()
min_distance = min(lidar_distances)
if min_distance < self.stop_distance:#当小于最小值时停止
if turtlebot_moving:
twist.linear.x = 0.0
twist.angular.z = 0.0
self._cmd_pub.publish(twist)
turtlebot_moving = False
rospy.loginfo('Stop! Distance of the obstacle : %f', min_distance)
break
else:
twist.linear.x = self.speed_linear
twist.angular.z = 0.0
self._cmd_pub.publish(twist)
turtlebot_moving = True
rospy.loginfo('Distance of the obstacle : %f', min_distance)
self.rate.sleep()
def main():
rospy.init_node('move_obstacle')
try:
obstacle = Obstacle()
except rospy.ROSInterruptException:
pass
if __name__ == '__main__':
main()
sensor_msgs/LaserScan Message 的定义如下：
Header header
float32 angle_min #开始角度（rad）
float32 angle_max #结束角度（rad）
float32 angle_increment #相邻两个取点方向的角度增量（rad）
float32 time_increment #相邻两个取点方向的时间增量（s）
float32 scan_time #完成一圈扫描所需的时间（s），除以time_increment可得一圈扫描点个数
float32 range_min #开始距离（m）
float32 range_max #结束距离（m）
float32[] ranges #数值介于开始距离与结束距离之间的距离（m)
float32[] intensities #强度数据
nav_obstacle.py 的精髓如下：
# 精髓
if scan_filter[i] == float('Inf') or math.isnan(scan_filter[i]):
scan_filter[i] = 10
# 解析
# 如果雷达扫描到的 介于开始距离和结束距离之间的距离列表 中 下标为i的距离数值 为无穷大或者不是一个数字，则将距离直接赋为10m
5.3 nav_square.py
nav_square.py 的源码如下：
#!/usr/bin/env python
# coding:utf-8
import rospy
from geometry_msgs.msg import Twist, Point, Quaternion
import tf
from math import radians, copysign, sqrt, pow, pi
import PyKDL
def quat_to_angle(quat):
rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
return rot.GetRPY()[2]
def normalize_angle(angle):
res = angle
while res > pi:
res -= 2.0 * pi
while res < -pi:
res += 2.0 * pi
return res
class NavSquare():
def __init__(self):
# 设置rospy在终止节点时执行的关闭函数
rospy.on_shutdown(self.shutdown)
# 获取参数
self.goal_distance = rospy.get_param("~goal_distance", 1.0) # meters
self.goal_angle = radians(rospy.get_param("~goal_angle", 90)) # degrees converted to radians
self.linear_speed = rospy.get_param("~linear_speed", 0.2) # meters per second
self.angular_speed = rospy.get_param("~angular_speed", 0.7) # radians per second
self.angular_tolerance = radians(rospy.get_param("~angular_tolerance", 2)) # degrees to radians
self.velocity_topic = rospy.get_param('~vel_topic', '/mobile_base/mobile_base_controller/cmd_vel')
self.rate_pub = rospy.get_param('~velocity_pub_rate', 20)
self.rate = rospy.Rate(self.rate_pub)
# 初始化发布速度的topic
self.cmd_vel = rospy.Publisher(self.velocity_topic, Twist, queue_size=1)
self.base_frame = rospy.get_param('~base_frame', '/base_link')#set the base_frame
self.odom_frame = rospy.get_param('~odom_frame', '/odom')#set the odom_frame
# 初始化tf
self.tf_listener = tf.TransformListener()
rospy.sleep(2)
try:
self.tf_listener.waitForTransform(self.base_frame,
self.odom_frame,
rospy.Time(0),
rospy.Duration(5.0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logerr('tf catch exception when robot waiting for transform......')
exit(-1)
# 循环四次画出正方形
for i in range(4):
self.Linear_motion()
# 在进行旋转之前使机器人停止
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1.0)
self.Rotational_motion()
# 在进行下一次线性运动之前使机器人停止
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1.0)
#结束时使机器人停止
self.cmd_vel.publish(Twist())
# 线性运动的具体工作
def Linear_motion(self):
position = Point()
move_cmd = Twist()
move_cmd.linear.x = self.linear_speed
# Get the starting position values
(position, rotation) = self.get_odom()
x_start = position.x
y_start = position.y
distance = 0
# Enter the loop to move along a side
while distance < self.goal_distance and not rospy.is_shutdown():
# Publish the Twist message
self.cmd_vel.publish(move_cmd)
self.rate.sleep()
# Get the current position
(position, rotation) = self.get_odom()
# Compute the Euclidean distance from the start
distance = sqrt(pow((position.x - x_start), 2) +
pow((position.y - y_start), 2))
# 旋转运动的具体工作
def Rotational_motion(self):
(position, rotation) = self.get_odom()
move_cmd = Twist()
move_cmd.angular.z = self.angular_speed #赋值旋转角度
# Track the last angle measured
last_angle = rotation
turn_angle = 0
# Begin the rotation
while abs(turn_angle + self.angular_tolerance) < abs(self.goal_angle) and not rospy.is_shutdown():
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)
self.rate.sleep()
# Get the current rotation
(position, rotation) = self.get_odom()
# Compute the amount of rotation since the last lopp
delta_angle = normalize_angle(rotation - last_angle)
turn_angle += delta_angle
last_angle = rotation
# 获取odom和base_link之间的当前变换
def get_odom(self):
try:
(trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return
return (Point(*trans), quat_to_angle(Quaternion(*rot)))
# 当关闭节点时会运行的函数
def shutdown(self):
rospy.loginfo("Stopping the robot...")
self.cmd_vel.publish(Twist())
rospy.sleep(1)
if __name__ == '__main__':
try:
rospy.init_node('nav_square', anonymous=False)
NavSquare()
except rospy.ROSInterruptException:
rospy.loginfo("Navigation terminated.")
nav_square.py 的精髓如下：
# 精髓1
# 循环四次画出正方形
for i in range(4):
self.Linear_motion()
# 学习点1：在进行旋转之前使机器人停止
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1.0)
self.Rotational_motion()
# 学习点2：在进行下一次线性运动之前使机器人停止
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1.0)
# 精髓2
# 获取odom和base_link之间的当前变换
def get_odom(self):
try:
(trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return
return (Point(*trans), quat_to_angle(Quaternion(*rot)))
# 学习点：*trans / *rot
# 可以将不定数量的参数传递给函数，而不必展开写为 Point(trans.x, trans.y, trans.z) 和 Quaternion(rot.x, rot.y, rot.z, rot.w)
# 精髓3
def quat_to_euler(quat):
rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
return rot.GetRPY()[2]
# 学习点：利用PyKDL包，写简易代码转四元数为欧拉角
版权声明：本文为MuscleMonk原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。