Implementing PID to follow wall

edit

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 :

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.

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 ?