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:
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
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]