ros topic (part 1)

作业2.1

src: simple_vel_publisher.py

#! /usr/bin/env python

import rospy
from std_msgs.msg import Int32 
from geometry_msgs.msg import Twist

rospy.init_node('topic_publisher')
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
rate = rospy.Rate(10)
count = Twist()
count.linear.x=0
while not rospy.is_shutdown(): 
  pub.publish(count)
  count.linear.x+=0.01
  rate.sleep()

launch: simple_vel_publisher_launch.launch

<launch>
  <!-- turtlebot_teleop_key already has its own built in velocity smoother -->
  <node pkg="my_topic_publisher" type="simple_topic_publisher.py" name="topic_publisher"  output="screen">
  </node>
</launch>

在Python文件中，创建了一个叫topic_publisher的节点，该节点中有一个消息发布器，发布的消息为/cmd_vel话题。该消息类型为Twist,隶属于geometry_msgs.msg类。
Twist的数据结构如下,分别是三个线速度，三个角速度。

geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

ros topic (part 2)

part 1中讲述如何创建一个消息发送者。 part 2则讲述如何订阅消息，同时创建自己的消息。

例子1
我们创建一个消息订阅器

#! /usr/bin/env python

import rospy
from std_msgs.msg import Int32 

def callback(msg): 
  print msg.data

rospy.init_node('topic_subscriber')               #订阅器节点
sub = rospy.Subscriber('counter', Int32, callback)#订阅器生成，参数为1.话题；2.消息数据类型；3.回调函数
                                                  #回调函数的作用是将订阅到的消息解析并返回所需要的值
rospy.spin()

但需要注意的是，话题topic必须是已经发布过的，不然订阅不到，会返回如下错误：

WARNING: no messages received and simulated time is active.
Is /clock being published?

在使用指令

rostopic pub <topic_name> <message_type> <value>
#我们这里发布的是
rostopic pub /counter std_msgs/Int32 5

发布话题后，监听/counter消息可以发现返回了数字5.

user:~$ rostopic echo /counter
data: 5
---

作业1
订阅小车发布的里程计消息.
该消息发布在/odom话题上。首先看看这个话题中消息类型,可以看出是Odometry

$ rostopic info /odom
Type: nav_msgs/Odometry

Publishers:
 * /gazebo (http://ip-172-31-28-139:40165/)

Subscribers: None

Odometry的消息结构：

$ rosmsg show Odometry
[nav_msgs/Odometry]:
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose
  geometry_msgs/Pose pose
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
  float64[36] covariance
geometry_msgs/TwistWithCovariance twist
  geometry_msgs/Twist twist
    geometry_msgs/Vector3 linear
      float64 x
      float64 y
      float64 z
    geometry_msgs/Vector3 angular
      float64 x
      float64 y
      float64 z
  float64[36] covariance

我们写一个订阅器如下：

#! /usr/bin/env python

import rospy
from nav_msgs.msg import Odometry

def callback(msg):
    print msg.twist.twist.linear


rospy.init_node('topic_subscriber')
sub = rospy.Subscriber('/odom',Odometry,callback)

rospy.spin()

ros中消息一般是xml树形结构，返回信息需要逐层引用。比如，如果要获取geometry_msgs/Vector3 linear 那么就需要在回调函数中写 msg.twist.twist.linear

作业2
发布一个自创消息，小车年龄。
首先要知道如何自创消息。我们可以使用rosmsg list 所示所有消息类型，但是如何制作自己的消息呢？
简单来说有如下几步：

Create a directory named ‘msg’ inside your package
Inside this directory, create a file named Name_of_your_message.msg
Modify CMakeLists.txt file
Modify package.xml file
Compile
Use in code

下面就上述步骤提供一个例子进行详细解释。
我们要创建一种叫做Age.msg的消息类型。

在已创建的包内创建文件夹msg
在msg文件夹内创建文件Age.msg,内容如下：
```
float32 years
float32 months
float32 days
```

配置编译文件：CMakeLists.txt
为了添加新消息类型，我们需要修改CMakeLists.txt中如下四个函数：

find_package() #查找该项目所需要的依赖库

find_package(catkin REQUIRED COMPONENTS
   rospy
   std_msgs
   message_generation   # Add message_generation here, after the other packages
)

add_message_files()# 将消息文件.msg名字加入该函数

add_message_files(
  FILES
  Age.msg
) # Dont Forget to UNCOMENT the parenthesis and add_message_files TOO

generate_messages()

generate_messages(
DEPENDENCIES
std_msgs
)

catkin_package()

catkin_package(
CATKIN_DEPENDS rospy message_runtime
)

修改package.xml
主要是添加创建与运行时的包依赖。

<build_depend>message_generation</build_depend> 
<run_depend>message_runtime</run_depend>

编译
就是使用catkin_make 编译一下包，但需要注意，要先把原先生成的build devel文件删除(当然，要是使用增量编译，编译特定的包catkin_make --only-pkg-with-deps <package_name> 也可以，那样更快一些)
注意，不要将可执行的python文件名字命名得跟所在包一模一样，也就是要避免如同这样的命名：
```
python: my_topic.py
package: my_topic
```
这样会带来一个问题，在import消息的时候，它会去搜索my_topic.py，这显然不是个文件夹，里面也没有消息文件，这样就会出错。
这时可以使用命令
```
rosmsg show Age
```
来检测一下Age消息是否创建成功，成功的话，会输出如下：
```
user ~ $ rosmsg show Age
[my_topic/Age]:
float32 years
float32 months
float32 days
```

使用
最后当然是使用该消息了，比如下面就是个发布自制消息的例子：
src: age_publisher.py

import rospy
from my_topic.msg import Age
rospy.init_node('age_publisher')
pub = rospy.Publisher('/age', Age, queue_size=5)
rate = rospy.Rate(2)
count = Age()
count.years=2018
count.months=3
count.days=16
while not rospy.is_shutdown(): 
pub.publish(count)
rate.sleep()

launch:

<launch>
<!-- turtlebot_teleop_key already has its own built in velocity smoother -->
<node pkg="my_topic" type="age_publisher.py" name="age_publisher"  output="screen">
</node>
</launch>

在这个例子中创建了一个一直发送年龄的发布者节点，可以使用

rostopic  echo /age

来查看输出。

mini project
自动避障小车
基本结构：

首先订阅/kobuki/laser/scan消息,
对激光消息进行分析，从而给出速度控制率
创建一个速度发布者，将速度发布到/cmd_vel 节点

1. laser/scan消息解析

[sensor_msgs/LaserScan]:
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
float32 angle_min
float32 angle_max
float32 angle_increment
float32 time_increment
float32 scan_time
float32 range_min
float32 range_max
float32[] ranges
float32[] intensities

我们主要需要的是关键字ranges中的值，ranges是各条激光线检测出的各个角度上障碍物到小车的距离。因为小车的速度可以分解为线速度与角速度，其中线速度就是小车坐标系中指向前方的速度。同样，激光雷达的正前方与小车正前方一致。

2.控制策略
主要需要考虑线速度与角度的变化情况。
a. 为避免正面碰撞，初步设计 $L_{min}>2v\Delta t$ ,也就是让小车与障碍物的安全距离与当前速度与速度变化周期相关，并留有一定的余量。
b. 转向策略，第一种方案是保持线速度不变，转动一定角度，做 $L_{min}$ 判断，直到满足条件，则停止发布角速度；第二种方案，是停下来做转动，直到距离满足条件，则启动线速度。注意上述两种方案中在移动的时候都是恒定速度的。

一种简单的避障思路：
小车激光雷达扫描范围是 $[-\pi/2,\pi/2]$ ,所以可以以正前方，左右两侧共三个检测方向的最短距离作为避障判断依据。
如果小车正前方与障碍物距离小于固定值，则默认顺时针转动小车。为避免侧边碰撞，根据左右两侧与障碍物距离，左边离的近了，那么就往顺时针转避开；右边离得近了也是同理。

粗略代码如下：

#! /usr/bin/env python
import rospy
from nav_msgs.msg import Odometry
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist

def callback(msg):
    global l_ranges
    l_ranges=msg.ranges

if __name__=='__main__':
    l_ranges=None
    rospy.init_node('topic_subscriber')
    sub = rospy.Subscriber('/kobuki/laser/scan',LaserScan,callback)
    pub=rospy.Publisher('/cmd_vel',Twist,queue_size=4)
    while l_ranges is None:
        pass

    num=len(l_ranges)
    choose_num=10
    my_vel=Twist()
    my_vel.linear.x=0.1
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():

        avg=sum(l_ranges[(num-choose_num)/2:(num+choose_num)/2])/choose_num
        if avg<1.0:
            my_vel.linear.x=0
            my_vel.angular.z=0.2
        else:
            my_vel.linear.x=0.1
            my_vel.angular.z=0.0

        if l_ranges[num-1]<0.3:
            my_vel.linear.x=0
            my_vel.angular.z=-0.2

        if l_ranges[0]<0.3:
            my_vel.linear.x=0
            my_vel.angular.z=0.2

        pub.publish(my_vel)
        rate.sleep()

上述代码能实现躲避障碍的功能，但实际情况下，在避障的同时也需要做其他事情，如到达给定目标点，走指定轨迹，速度加速度约束；高速机动物体，如飞行器避障等复杂问题。

Robot Ignite2: ROS基础--话题(topic)

ros topic (part 1)

ros topic (part 2)

猜你喜欢