效果如何呢,请看下图:
跑一下看看:
启动环境如下:
<launch>
<include file="$(find stdr_robot)/launch/robot_manager.launch" />
<node type="stdr_server_node" pkg="stdr_server" name="stdr_server" output="screen" args="$(find ROBO)/resources/maps/z.yaml"/>
<node pkg="tf" type="static_transform_publisher" name="world2map" args="0 0 0 0 0 0 world map 100" />
<include file="$(find stdr_gui)/launch/stdr_gui.launch"/>
<node pkg="stdr_robot" type="robot_handler" name="$(anon robot_spawn_0)" args="add $(find ROBO)/resources/robots/simplex.yaml 1 4.5 0" />
<node pkg="stdr_robot" type="robot_handler" name="$(anon robot_spawn_1)" args="add $(find ROBO)/resources/robots/simplex.yaml 7 4 3.14" />
</launch>
沿墙跑有bug代码如下:
#!/usr/bin/env python
import sys
import rospy
import numpy as np
from math import cos, sin
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from sensor_msgs.msg import Range
np.set_printoptions(precision=2)
orbit = 0
laser_sensors = {'w': 0, 'nw': 0, 'n': 0, 'ne': 0, 'e': 0}
linear_vel = 0.1
angular_vel = 0.4
wall_distance = 0.4
wall_distance_forward = 0.30
wall_distance_side = 0.35
inf = float('inf')
left = -1
going_left = -2
right = 1
going_right = 2
def calculate_lasers_range(data):
'''Dynamic range intervals'''
global laser_sensors
half_pi = np.pi / 2
initial_angle = 0
final_angle = 0
if data.angle_min < -half_pi:
default_min_angle = half_pi / data.angle_increment
robot_initial_angle = -data.angle_min / data.angle_increment
initial_angle = robot_initial_angle - default_min_angle
if data.angle_max > np.pi / 2:
default_max_angle = half_pi / data.angle_increment
robot_final_angle = data.angle_max / data.angle_increment
final_angle = robot_final_angle - default_max_angle
laser_interval = (len(data.ranges) - initial_angle - final_angle) / 5
half_laser_interval = laser_interval / 2
interval = [None] * 5
interval[0] = np.mean(data.ranges[int(initial_angle):int(laser_interval)])
for i in range(1, 5):
dirty_values = data.ranges[int(
initial_angle + i * laser_interval - half_laser_interval
):int(initial_angle + i * laser_interval + half_laser_interval) + 1]
interval[i] = np.mean(np.nan_to_num(dirty_values))
laser_sensors['e'] = interval[0]
laser_sensors['ne'] = interval[1]
laser_sensors['n'] = interval[2]
laser_sensors['nw'] = interval[3]
laser_sensors['w'] = interval[4]
def log_info():
'''Initial orbit state'''
global orbit, laser_sensors
orbit_values = {-2: 'Going Left', -1: 'Left', 0: 'Undefined', 1: 'Right', 2: 'Going Right'}
rospy.loginfo("Orbit: %s, W : %s, NW: %s, N : %s, NE: %s, E : %s", orbit_values[orbit],
laser_sensors['w'], laser_sensors['nw'], laser_sensors['n'],
laser_sensors['ne'], laser_sensors['e'])
def create_velocity_message(turn_left, turn_right, forward):
angular = 0
linear = 0
if (turn_left):
angular += angular_vel
if (turn_right):
angular -= angular_vel
if (forward):
linear = linear_vel
vel_msg = Twist()
vel_msg.linear.x = linear
vel_msg.angular.z = angular
return vel_msg
def publish_velocity_message(vel_msg):
vel_pub = rospy.Publisher(
'/robot' + sys.argv[1] + '/cmd_vel', Twist, queue_size=10)
vel_pub.publish(vel_msg)
def laser_callback(data):
global orbit, laser_sensors
calculate_lasers_range(data)
log_info()
linear = 0
angular = 0
forward = False
turn_left = False
turn_right = False
if (orbit == 0):
if (laser_sensors['w'] < wall_distance_side):
orbit = left
elif (laser_sensors['e'] < wall_distance_side):
orbit = right
elif (laser_sensors['nw'] < wall_distance):
orbit = going_left
turn_right = True
elif (laser_sensors['ne'] < wall_distance):
orbit = going_right
turn_left = True
elif (laser_sensors['n'] < wall_distance_forward):
orbit = going_left
turn_right = True
else:
forward = True
elif (orbit == going_left or orbit == going_right):
if (laser_sensors['w'] < wall_distance_side):
orbit = left
elif (laser_sensors['e'] < wall_distance_side):
orbit = right
elif (orbit == going_left):
turn_right = True
elif (orbit == going_right):
turn_left = True
elif (orbit == left):
if (laser_sensors['n'] > wall_distance_forward
and (laser_sensors['w'] > wall_distance_side
or laser_sensors['e'] > wall_distance_side)):
forward = True
if (laser_sensors['w'] <= wall_distance_side
and laser_sensors['e'] <= wall_distance_side):
turn_right = True
elif (laser_sensors['nw'] <= wall_distance
or laser_sensors['ne'] <= wall_distance):
turn_right = True
else:
if (laser_sensors['ne'] < wall_distance
or laser_sensors['nw'] < wall_distance
or laser_sensors['n'] < wall_distance_forward):
turn_right = True
else:
turn_left = True
elif (orbit == right):
if (laser_sensors['n'] > wall_distance_forward
and (laser_sensors['w'] > wall_distance_side
or laser_sensors['e'] > wall_distance_side)):
forward = True
if (laser_sensors['w'] <= wall_distance_side
and laser_sensors['e'] <= wall_distance_side):
turn_left = True
elif (laser_sensors['nw'] <= wall_distance
or laser_sensors['ne'] <= wall_distance):
turn_left = True
else:
if (laser_sensors['ne'] < wall_distance
or laser_sensors['nw'] < wall_distance
or laser_sensors['n'] < wall_distance_forward):
turn_left = True
else:
turn_right = True
vel_msg = create_velocity_message(turn_left, turn_right, forward)
publish_velocity_message(vel_msg)
def sonar_callback(data):
pass
def listeners():
rospy.Subscriber('/robot' + sys.argv[1] + '/laser_0', LaserScan,
laser_callback)
rospy.Subscriber('/robot' + sys.argv[1] + '/sonar_0', Range,
sonar_callback)
rospy.spin()
if __name__ == '__main__':
rospy.init_node('avoid_wall_' + sys.argv[1], anonymous=True)
listeners()
基础概念如下:
机器人可以像沿着路径那样沿着墙行进。和路径一样,墙也为机器人提供了导航方向。这种方法的好处在于不用画任何路线或放置带子。根据机器人的设计,甚至可以绕过小的障碍物(门墙的距离。不需要和墙壁有物理上的接触。在典型的非接触系统中,通常还要用两个传感器判断机器人是否与墙平行。 (1)非接触式,无源传感器:采用无源传感器,例如用霍尔效应开关来判断与固定的墙壁的距离。在使用霍尔效应开关的情况下,可以将一根通有低压交流电流的电线安装在护壁板或墙上。当机器人靠近时传感器会收到交流电产生的感应磁场。或者,如果护壁板是金属做的,霍尔效应开关(当在板的反面装一块小磁铁时)能够探测到接近了墙壁。 (2)“软接触”:机器人采用柔软的材料通过机械手段探测与墙接触。比如说,把一个很轻的橡胶轮当作“墙上滚轴”,如下图所示。软接触的好处是由于采用塑性或柔软的介质,能减少或消除机械失误。在上述情况下,当机器人遇到墙时,就会启动一个控制程序以便沿着墙到达目的地。在一个简单的接触系统中,机器人可能在接触墙壁之后退回一点,然后再向着墙绕一个大的弧线。这个过程不断重复,最后的作用就是让机器人“沿着墙壁行进”。 对于其他方法,优先采用的方式是让机器人维持与墙壁的适当距离。一旦邻近的墙壁丢失了,就进入“寻找墙壁”模式。这将使机器人以弧线朝着预期的墙的方向行进。一旦接触后,便稍微地改变路线开始一个新的弧线。典型的运动形式可参看下图。
2.用超声波探测沿墙行进 简单的超声波墙壁跟随法可以使用两对超声波发射器/接收器。为了避免干扰,每个发射器和接收器将相隔几英寸。它们用来帮助机器人平行于墙壁行走。正确的超声波发射器和接收器电路在第“碰撞的避免及探测”一讲中有详细说明。 由于机器人一般离墙壁很近(相差几英寸),你应该用非常低功率的发射器且只使用中等放大器,可能的话,接收器也一样。可以通过降低电压以降低发射器的驱动功率。 3.用橡胶轮子的软接触跟踪 用滚轮软接触沿墙行进提供你一些有趣的可能性。事实上,通过在机器人手臂上放置用软橡胶做成的空滚轮,然后使机器人不断的朝着墙壁内侧行驶,可以大幅度简化传感器和控制电路。朝内侧行驶通过使机器人内侧的轮子(在墙壁的边上轮子)稍微慢于另外一只就很容易做到。橡胶滚轮将会阻止机器人和墙壁碰撞。 4.关于门口和物体 实质上,只沿着一面墙壁行走并不算困难。而希望机器人绕过障碍物或越过门口的时候,工作变得更具挑战性。 这就需要附加的传感器,比如机器人前部的触须或其他的触觉传感器。这些传感器用来探测墙角。当你正在构造一个沿着墙壁简单地向内弧形行进的机器人时,这显得尤其重要。如果不能感觉到正前方的墙壁,机器人可能会绝望地被困在一个角落中。 使用一个较长距离的超声波传感器,可以感应到通向其他房间的敞开的门口。在这里,长距离的超声波探测机器人距离两边的墙壁有多远,并且将其设为“直行”模式。理论上,机器人应该计算这一模式的持续时间以便估计与敞开的门口的最大距离。如果在×秒内没有探测到墙壁,机器人应该进入“寻找墙壁”模式。