ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org
Ask Your Question
0

Implementing PID to follow wall

asked 2021-04-21 10:50:44 -0500

sisko gravatar image

updated 2021-04-21 22:55:25 -0500

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 ?

edit retag flag offensive close merge delete

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

jayess gravatar image jayess  ( 2021-04-21 15:47:37 -0500 )edit

@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.

sisko gravatar image sisko  ( 2021-04-21 18:34:14 -0500 )edit

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.

tryan gravatar image tryan  ( 2021-04-21 19:44:56 -0500 )edit

@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

sisko gravatar image sisko  ( 2021-04-21 22:51:41 -0500 )edit

Run

rostopic info /cmd_vel

to see what other nodes are publishing to that topic

jayess gravatar image jayess  ( 2021-04-21 23:29:42 -0500 )edit

@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.

sisko gravatar image sisko  ( 2021-04-22 05:33:50 -0500 )edit

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.

tryan gravatar image tryan  ( 2021-04-22 08:29:29 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2021-06-06 09:51:45 -0500

Sachin Thakur gravatar image

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)

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2021-04-21 10:50:44 -0500

Seen: 554 times

Last updated: Jun 06 '21