Robotics StackExchange | Archived questions

Implementing PID to follow wall

My problem is 2-fold.

1 :

I am using move_base and successfully getting my robot model to autonomously navigate from one pose to another in a gazebo world. However, I am looking for a way to finetune the robots' path. Specifically, I want the model to travel close to the left or right wall, depending on which side I specify, maintaining a consistent predefined gap between the wall and robot.

I minimised my gazebo world to the following :

image description

The laser currently has a distnce of 194.374211311 between the wall and the left side of the robot. I need the robot to travel alongside the wall with a seperating distance of 25. I tried accomplishing this with the following code :

#!/usr/bin/env python

import rospy
import math
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
import sys

class WallTracker :

    data_left = 0
    cdm_vel_publisher = ''

    def __init__(self):
        rospy.init_node('laser_ranging', anonymous=False)
        self.cdm_vel_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

        rospy.Subscriber('scan_left', LaserScan, self.sample)

        rospy.Subscriber("cmd_vel_original", Twist, self.move)

        rospy.spin()

    def sample(self, data):
        cleanedData = filter(lambda x: not math.isinf(x), data.ranges)
        self.data_left = sum(cleanedData)

    def move(self, twist):
        rospy.loginfo('RANGING LEFT : ' + str(self.data_left))
        rospy.loginfo(twist)
        new_twist = Twist()
        new_twist = twist
        rospy.loginfo(new_twist)

        limit = 20

        # Also tried ...
        new_twist.linear.x = twist.linear.x
        new_twist.linear.y = twist.linear.y
        new_twist.linear.z = twist.linear.z
        new_twist.angular.x = twist.angular.x
        new_twist.angular.y = twist.angular.y
        # new_twist.angular.z = twist.angular.z

        if(self.data_left > limit):
            new_twist.angular.z = 1
            rospy.loginfo('Data-Left['+str(self.data_left)+'] > ' + str(limit))
        else:
            new_twist.angular.z = 0
            rospy.loginfo('Data-Left['+str(self.data_left)+'] < ' + str(limit))

        self.cdm_vel_publisher.publish(new_twist)

if __name__ == "__main__":
    WallTracker()

I am basically trying to amend the angular.z value of the twist from cmd_vel. The idea is to allow the robot to continue it's movement while turning it to the left and bringing it closer to the wall.

BUT, I find the robot only turns in place.

image description

From the terminal output, it seems my attempt to duplicate the twist data as generated by move_base has failed and only my angular.z data is in my newly created and published twist data.

Echoing out the cmd_vel topic bears me out. When I commentout the entire if-statement, I have linear.x data coming from move_base. But with the if-statement that linear.x value is absent.

2 :

How can I implement a PID to get the robot model to accurately drive alongside the wall ?

Asked by sisko on 2021-04-21 10:50:44 UTC

Comments

Are you updating data_left? If it's always spinning then it seems to be always greater than limit. Is this the entirety of the code that you're using? If not, please update your question with all of the code that you're using

Asked by jayess on 2021-04-21 15:47:37 UTC

@jayess: data_left is the total of all the numbers on the scan_left topic having removed all inf occurances. The closer the robot moves towards the wall, the smaller data_left becomes.

Asked by sisko on 2021-04-21 18:34:14 UTC

The twist messages in the GIF seem to be constant and have angular.z = 1--even at the end, when the robot stops turning. It seems that value isn't coming from your code. Do you know what's generating it?

On a side note, you should probably not create the Publisher() in move() if you're calling it often. Usually, one would create the Publisher() as a member of a class when initializing the node. Then other class methods can publish as needed.

Asked by tryan on 2021-04-21 19:44:56 UTC

@Tryan: Thanks. I refactored my code into a class so the publisher object is created as the instance of my class is being initiated. As for the angular.z=1 issue, I noticed it but I honestly don't know where it coming from. It does seem to be coming from the same code however. If I commen out my if-statement and the preceeding twist assignement code, I don't get angular.z = 1

Asked by sisko on 2021-04-21 22:51:41 UTC

Run 

rostopic info /cmd_vel

to see what other nodes are publishing to that topic

Asked by jayess on 2021-04-21 23:29:42 UTC

@jayess: Only my node is publishing to cmd_vel. This is VERY strange because the else section of my if statement should be setting angular.z to 0. But I am now getting -1 or 1 as observered by @Tryan.

Asked by sisko on 2021-04-22 05:33:50 UTC

twist messages in the GIF seem to be constant and have angular.z = 1

I see now. rospy.loginfo(new_twist) is printing new_twist = twist before you modify it, so it seems that's just the value coming from /cmd_vel. You can echo /cmd_vel directly to confirm. If that is the case, then there's no non-zero components besides angular.z, so I would expect your robot to spin like it's doing. Further, in the GIF, the robot does stop when the limit is reached, so it's probably getting the correct message (angular.z = 0). All that's left then is to figure out why /cmd_vel isn't what you think it should be.

Asked by tryan on 2021-04-22 08:29:29 UTC

Answers

I am new to ROS myself but in past, i have made a wall following robot using arduino that used PID controller. There are two things to consider- 1) if you are using a single ultrasound sensor which is not directly perpendicular to the wall, there is a chance that the sent pulse might not even return since it bounced off at an angle(law of reflection at an inclined surface). This is the reason why 2D lidars are better suited at mapping walls.

2) if you are using a sensor which is placed perpendicular to the wall, you should be able to write a PID script using the sensor input. Let's say you get the data on "/Laser" topic and you want to keep your bot at a certain distance x form the wall. So now, you have the goal signal, i.e. x and the position signal, i.e. /Laser. now you can use pid ros package to either tune the bot manually using dynamic reconfiguration or use the autotune module for ros-pid package. Link- http://wiki.ros.org/pid

The method discussed here will allow the bot to maintain a certain distance from the wall but if you want to keep it moving along the wall, you have to give additional rewards like increase the x pose of the bot. This can still lead to oscillations since you are using only one ultrasound sensor. If you want finer results, use two sensors. One at the front of the bot and other at the back of the bot, both facing the wall. (allow keep a small error window, else the bot might oscillate)

Asked by Sachin Thakur on 2021-06-06 09:50:35 UTC

Comments