ROS_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