ROS，机器人右转问题

Question

我的 Husarion ROSbot 倾向于向右偏离路线。有什么办法可以在软件中更正。我希望 ROSbot 以相等的距离行进，或者在与墙壁平行的某个范围内行进。到目前为止，我尝试根据激光雷达读数发布对 /cmd_velosity 的课程调整。

当我使用以下代码时，机器人会做出错误的更正。

import rospy
import time
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
​
forward_object_distance = 2.0
backward_object_distance = 2.0
​
wall_90_distance = 0
wall_270_distance = 0
​
def callback(msg):
    global forward_object_distance
    global wall_90_distance
    global wall_270_distance
​
    if not np.isinf(msg.ranges[90]):
        wall_90_distance = msg.ranges[90]
    if not np.isinf(msg.ranges[270]):    
        wall_270_distance = msg.ranges[270]
    if not np.isinf(msg.ranges[0]):
        forward_object_distance = msg.ranges[0]

rospy.init_node('move_robot_node')
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
sub = rospy.Subscriber('/scan', LaserScan, callback)
​
def go_forward(_rate, _velocity, _distance):
    global wall_270_distance
    global wall_90_distance
    global i
    rate = rospy.Rate(_rate)
    move = Twist()
    move.linear.x = _velocity
    while forward_object_distance > 0.2:
        if wall_90_distance <= 0.5:
            move.angular.z = -0.1
        elif wall_90_distance >= 0.6:
            move.angular.z = 0.1
        move.angular.z = 0.0
        pub.publish(move)
        rate.sleep()
​
    move.linear.x = 0.0
    move.angular.z = 0.0
    pub.publish(move)
​    ​    ​
go_forward(40, 0.3, 0.5)
​
move = Twist()
move.linear.x = 0.0
move.angular.z = 0.0
pub.publish(move)

Answer 1

这个问题在 Husarion 论坛上讨论并解决： topic 1 和 topic 2，下面是最终解决方案：

通过仅检查来自激光扫描仪的一个值，您只有在 ROSbot 与墙壁平行放置时才能获得正确的测量结果。当 ROSbot 转向以靠近墙壁行驶时，您的测量值将增加 1/cos(α)，其中 α 是 ROSbot 和墙壁之间的角度。 ROSbot 越是转向墙壁，您使用的值越不正确。

由于激光雷达可提供更多数据，因此最好使用它们，例如。点与机器人成 90 度和 70 度，然后计算墙壁距离和方向。

import rospy
import time
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from math import atan, cos, sin, radians, degrees
​
forward_object_distance = 2.0
backward_object_distance = 2.0
​
wall_90_distance = 0
wall_70_distance = 0
wall_270_distance = 0
​
def callback(msg):
    global forward_object_distance
    global wall_70_distance
    global wall_90_distance
    global wall_270_distance

    if not np.isinf(msg.ranges[70]):
        wall_70_distance = msg.ranges[70]​
    if not np.isinf(msg.ranges[90]):
        wall_90_distance = msg.ranges[90]
    if not np.isinf(msg.ranges[270]):    
        wall_270_distance = msg.ranges[270]
    if not np.isinf(msg.ranges[0]):
        forward_object_distance = msg.ranges[0]

def calculate_angle(l, l1, alfa):
    angle = atan((l1 * cos(radians(alfa)) - l) / (l1 * sin(radians(alfa))))
    return degrees(angle)

def calculate_wall_distance(l, alfa):
    distance = l * cos(radians(alfa))
    return distance

rospy.init_node('move_robot_node')
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
sub = rospy.Subscriber('/scan', LaserScan, callback)
​
def go_forward(_rate, _velocity, _distance):
    global wall_270_distance
    global wall_90_distance
    global i
    rate = rospy.Rate(_rate)
    move = Twist()
    move.linear.x = _velocity
    while forward_object_distance > 0.2:
        wall_angle = calculate_angle(wall_90_distance, wall_70_distance, 20)
        wall_distance = calculate_wall_distance(wall_90_distance, wall_angle)
        if wall_distance <= 0.5:
            move.angular.z = -0.1
        elif wall_distance >= 0.6:
            move.angular.z = 0.1
        else:
            move.angular.z = 0.0
        pub.publish(move)
        rate.sleep()
​
    move.linear.x = 0.0
    move.angular.z = 0.0
    pub.publish(move)
​    ​    ​
go_forward(40, 0.3, 0.5)
​
move = Twist()
move.linear.x = 0.0
move.angular.z = 0.0
pub.publish(move)

Answer 2

我相信按照你纠正课程的逻辑，这部分代码应该更新：

if wall_90_distance <= 0.5:
  move.angular.z = -0.1
elif wall_90_distance >= 0.6:
  move.angular.z = 0.1
move.angular.z = 0.0

到

if wall_90_distance <= 0.5:
  move.angular.z = -0.1
elif wall_90_distance >= 0.6:
  move.angular.z = 0.1
else:
  move.angular.z = 0.0

否则，您不是每次都只是发布0个速度，并且由于累积的错误而导致偏差吗？