0

My Husarion ROSbot tends to deviate from the course to the right. Is there any way to correct it in the software. I would like ROSbot to travel at an equal distance, or in some range from a wall, parallel to it. So far I tried to publish course adjustments to /cmd_velosity, based on lidar readings.

When I use the following code, the robot makes incorrect corrections.

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)
quarkpol
  • 485
  • 1
  • 5
  • 10

2 Answers2

1

I believe following your logic of correcting the course, this part of the code should be updated:

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

to

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

Otherwise, wouldn't you be just publishing 0 velocities every time and it is just deviating due to accumulated errors?

Saisai3396
  • 311
  • 1
  • 8
1

This problem was discussed and solved on Husarion forum: topic 1 and topic 2, below is end solution:

By checking only one value from laser scanner, you will get correct measurement only with ROSbot positioned parallel to wall. When ROSbot turns in order to drive closer to wall, your measurement will be increased by 1/cos(α) where α is angle between ROSbot and wall. The more ROSbot is turning towards the wall, the more incorrect value you are using.

As the lidar provide much more data it would be better to use them, eg. points at 90 degrees from robot and 70 degrees, then calculate wall distance and orientation from it.

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)
Łukasz Mitka
  • 11
  • 1