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