[Solved] ROS, problem with robot which turns right

[quarkpol]

Hi,

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)

Best regards,
Quarkpol

[lukasz.mitka]

Hello Quarkpol,

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 ROSbot and wall. The more ROSbot is turning towards the wall, the more incorrect value you are using. I would suggest you to use more values, eg. 20 points from lidar and extract wall distance and orientation from it.

Regards,
Łukasz

[quarkpol]

Thank you, it helped a lot. If somebody will need code, I will share it with pleasure.

Regards,
Quarkpol