ROS_Python编程 之 案例代码核心解析(第四版)

  • Post author:
  • Post category:python

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 版权协议,转载请附上原文出处链接和本声明。