蓝桥ROS机器人之STDR沿墙跑

Posted 2022-05-31 zhangrelay

效果如何呢，请看下图：

跑一下看看：

---

启动环境如下： 

<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．关于门口和物体
　　
　　实质上，只沿着一面墙壁行走并不算困难。而希望机器人绕过障碍物或越过门口的时候，工作变得更具挑战性。
　　
　　这就需要附加的传感器，比如机器人前部的触须或其他的触觉传感器。这些传感器用来探测墙角。当你正在构造一个沿着墙壁简单地向内弧形行进的机器人时，这显得尤其重要。如果不能感觉到正前方的墙壁，机器人可能会绝望地被困在一个角落中。
　　
　　使用一个较长距离的超声波传感器，可以感应到通向其他房间的敞开的门口。在这里，长距离的超声波探测机器人距离两边的墙壁有多远，并且将其设为“直行”模式。理论上，机器人应该计算这一模式的持续时间以便估计与敞开的门口的最大距离。如果在×秒内没有探测到墙壁，机器人应该进入“寻找墙壁”模式。

---

 

开发者涨薪指南 48位大咖的思考法则、工作方式、逻辑体系