pythonrosrospy

How do I make dynamic reconfigure have the ability to change the angles at which my lidar can see?


I am trying to make a simulated robot only be able to see the 180 degrees in front of it with a lidar and I'm using dynamic reconfigure to control the angles/window that it should be able to detect objects within but I'm struggling to figure out how to implement that ability in my code.

Dynamic Reconfigure that I set up: enter image description here

Snippets of code that I'm using:

    # Initialize dynamic reconfigure
    self.enable = 0
    self.speed = 0.0
    self.window=60
    self.yaw_rate=0

    # Define the image subscriber
    self.sub_lidar = rospy.Subscriber('scan', LaserScan, 
                                      self.lidar_callback, queue_size=1)
 ################################
# Dynamic Reconfigure callback
################################
    def dyn_reconfig_callback(self, config, level):
        self.enable = config['enable']
        self.speed = config['speed']
        self.window= config['window']
        self.dyn_config = config
        return config
    


#########################
# Lidar image callback
#########################
    def lidar_callback(self, data):
        data.angle_max=(self.window*np.pi/180)/2
        data.angle_min=(self.window*np.pi/180)/-2
        min_idx=None
        min_dist=data.range_max
        for idx in range(len(data.ranges)):
            if(data.ranges[idx]>data.range_min and data.ranges[idx]<min_dist):
                min_idx=idx
                min_dist=data.ranges[idx]

        if(min_idx is not None):
            rospy.loginfo('closest object is at %.2f deg and dist = %.3f '%(min_idx*data.angle_increment*180/np.pi, min_dist))
        

            msg=Twist()
            msg.angular.z=min_idx*data.angle_increment
            self.pub_twist.publish(msg)
            if(min_dist<.30):
                self.enable=False

What I tried:

        data.angle_max=(self.window*np.pi/180)/2
        data.angle_min=(self.window*np.pi/180)/-2

Solution

  • If you ok with publishing lidar data for filtering you could use LaserScanAngularBoundsFilter


    Or manually:

    From LaserScan.msg data.angle_max and data.angle_min doesn't seem to discard measurements. That must be done manually. For example to keep it simple you have lidar which scans from 0 to 360 deg (angle_min = 0, angle_max = 2*pi) at 1 deg increments and you want to limit it to [90 .. 270 deg] range. You would need to not use 25% from the start and end of float32[] ranges array

    for range(90, 270+1):
        # Do your  data.ranges[i]  calculations here with limited angle range
    

    To generalize you would want to implement function which converts degrees to data.ranges[] index. If we assume that angle_max=2*pi and angle_min=0 that would be something like this (pseudo code):

    ind_start = int(len(data.ranges) * (your_angle_min / (2 * pi)))
    ind_end = int(len(data.ranges) * (your_angle_max / (2 * pi)))
    for range(ind_start, ind_end):
        # data.ranges[i] is filtered to contain ranges from angles [your_angle_min .. your_angle_max]