ROS机器人编程实践——读书笔记1

目的：写一个最小基于ROS的机器人控制软件

一、写一个运动命令流，每秒10次，每三秒启动一次。在移动时，发送前进命令，速度0.5米每秒，停止时发送速度0米每秒。命名为

red_light_green_light.py

 

 

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist

cmd_vel_pub=rospy.Publisher('cmd_vel',Twist,queue_size=1)
rospy.init_node('red_light_green_light')

red_light_twist=Twist()
green_light_twist=Twist()
green_light_twist.linear.x=0.5

driving_forward=True
light_chang_time=rospy.Time.now()
rate=rospy.Rate(10)
while not rospy.is_shutdown():
	if driving_forward:
		cmd_vel_pub.publish(green_light_twist)
	else:
		cmd_vel_pub.publish(red_light_twist)
	if light_chang_time > rospy.Time.now():
		driving_forward=not driving_forward
		light_chang_time=rospy.Time.now()+rospy.Duration(3)
	rate.sleep()

（1）queue_size=1，参数告诉rospy只缓冲一个消息。一旦节点发送的速率超过接收的频率，rospy就会将queue_size之外的消息扔掉。

 

（2）Twist消息中的linear（线性）速度的x分量。以0.5米每秒向前走。

（3）rospy.sleep（），代码1将继续进行。

接下来是测试：

1.设置为可运行的文件，在当前目录下 。$ chmod +x red_light_green_light.py

2.启动Gazebo，安装   $ sudo apt-get install ros-indigo-turtlebot-gazebo

3 .$ roslaunch turtlebot_gazebo turtlebot_world.launch

4.启动控制节点 $./red_light_green_light.py cmd_vel:=cmd_vel_mux/input/teleop

二、读取传感器数据

 Turtlebot深度传感器（Kinect）滤波之后以sensor_msgs/LaserScan的消息的形式发布到scan的话题上，但是该相机视角窄，最大检测距离很小，几乎是只能检测前面的障碍物，检测不到右侧的。这也是低成本深度相机的代价。

 $ roslaunch turtlebot_gazebo turtlebot_world.launch

 $ rostopic echo scan

读取的数据，这是通过topic响应的。

写一个完整的节点，这个节点输出正前方的障碍物到他的距离。

range_ahead.py

 

#!/usr/bin/env  python
import rospy
from sensor_msgs.msg import LaserScan
def scan_callback(msg):
	range_ahead=msg.ranges[len(msg.ranges)/2]
	print "range_ahead :%0.1f"%range_ahead

rospy.init_node('range_ahead')
scan_sub=rospy.Subscriber('scan',LaserScan,scan_callback)
rospy.spin()

可以在Gazebo里拖动turtlebot显示数据。
三、写一个一和二的结合。

 

turtlebot往前走直到看到与它小于0.8m的障碍物，运行大于30秒，会旋转到一个新方向。

 

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
def scan_callback(msg):
	global g_range_ahead
	g_range_ahead=min(msg.ranges)

g_range_ahead=1

sacn_sub=rospy.Subscriber('scan',LaserScan,scan_callback)
cmd_vel_pub=rospy.Publisher('cmd_vel',Twist,queue_size=1)
rospy.init_node('wander')
state_change_time=rospy.Time.now()
driving_forward=True
rate=rospy.Rate(10)

while not rospy.is_shutdown():
	if  driving_forward:
		if(g_range_ahead<0.8 or rospy.Time.now() > state_change_time):
			driving_forward=False
			state_change_time=rospy.Time.now()+rospy.Duration(5)
	else:
		if rospy.Time.now()>state_change_time:
			driving_forward=True
			state_change_time=rospy.Time.now()+rospy.Duration(30)
	twist=Twist()
	if driving_forward:
		twist.linear.x=1
	else:
		twist.angular.z=1
	cmd_vel_pub.publish(twist)

	rate.sleep()

1.$ chmod +x wander.py

 

2.启动控制节点 $./wander.py cmd_vel:=cmd_vel_mux/input/teleop2.