How to add the laser_filter to the code that finds the closest object distance?

asked 2018-03-07 05:12:31 -0500

Mekateng gravatar image

Hi, I wrote the nearest object distance code that called code.py. But ı want to add LaserScanAngularBaundsFilter to this code. I mean, Since there is no filtering, it always shows the floor as the closest object. like this ( https://i.hizliresim.com/G9W71r.png ) I need to do this filtering to solve this problem. So the points between the two angles need to be erased.How can ı do this?
code.py:

#!/usr/bin/env python

import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Point, PoseStamped, PointStamped
import tf2_ros

import PyKDL
import tf2_geometry_msgs
from math import cos,sin


class LaserRot(object):
    def __init__(self):
        self.laser=LaserScan()
        self.laserS=rospy.Subscriber("/scan", LaserScan, self.laser_callback)
        self.closestP=rospy.Publisher("/closest_point", PointStamped, queue_size=1)
    self.tf_buffer=tf2_ros.Buffer(rospy.Duration(1200.0))
    self.tf_listener=tf2_ros.TransformListener(self.tf_buffer)
    self.get_transform()

def get_transform(self):
    try:
        self.transform = self.tf_buffer.lookup_transform("base_link", "laser", rospy.Time(0), rospy.Duration(1.0))
    except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
        rospy.logerror("Error getting transform")
        print "Error"



def laser_callback (self, msg):
        self.laser=msg
        self.get_transform()

    def publish_closest_obstacle(self):
        laser = self.laser.ranges
        shortest_laser = 2.6
        mintest_laser=1.95
        point=Point()


for i in range(len(laser)):
                if laser[i] < shortest_laser and laser[i]>mintest_laser:
                    angle=self.laser.angle_min + i*self.laser.angle_incr

ement
                x=laser[i]*cos(angle)
                print ("laser: ", laser[i], "angle: ", angle*57.2958)
                if x>-0.2:
                    shortest_laser=laser[i]


point.x=x
                        point.y=shortest_laser*sin(angle)
        pose=PoseStamped()
        pose.header=self.laser.header
        point.z=0.0
        pose.pose.position=point
        pose_transformed= tf2_geometry_msgs.do_transform_pose(pose, self.transform)
        point_transformed=PointStamped()
        point_transformed.header=pose_transformed.header


point_transformed.point = pose_transformed.pose.position
            self.closestP.publish(point_transformed)

rospy.init_node("compute_closest_obstcl")
r=rospy.Rate(10)
lr=LaserRot()

while not rospy.is_shutdown():
    lr.publish_closest_obstacle()
    r.sleep()

my yaml. file in config package:

scan_filter_chain:
name:angular_bounds
 type: LaserScanAngularBoundsFilter
  params:
     lower_angle:-0.87
     upper_angle:0.87
edit retag flag offensive close merge delete