Ask Your Question
0

How to get the closest obstacle with rotating laser using pointcloud

asked 2017-12-20 07:21:51 -0600

Mekateng gravatar image

updated 2017-12-20 14:03:21 -0600

jayess gravatar image

Hi, The following code allows a rotating laser scanner to detect the object closest to itself using the LaserScan(scan topic) message.But ı want to this using pointcloud . How can I do this? How do we make a change in the code below for this? thanks in advance.

#!/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 = 10000
        point=Point()
        for i in range(len(laser)):
            if laser[i] < shortest_laser:
                angle=self.laser.angle_min + i*self.laser.angle_increment
                x=laser[i]*cos(angle)
                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()
edit retag flag offensive close merge delete

Comments

Do you have any idea Mr. @bvbdort ?

Mekateng gravatar imageMekateng ( 2017-12-20 07:28:46 -0600 )edit

1 Answer

Sort by » oldest newest most voted
0

answered 2017-12-20 18:15:27 -0600

You could simply convert the laser scan into a point cloud using the ros_pcl transforms package. Once in a pointcloud form, you can look at the L2 distance from the origin frame to determine which point is closest. Hope that helps. S

edit flag offensive delete link more

Comments

I converted laserscan to pointcloud Mr. @smac . Using the pointcloud form , How do we organize this code? so I ask how do you do something in the code.

Mekateng gravatar imageMekateng ( 2017-12-21 02:43:43 -0600 )edit

I trust that once you look at the ros_pcl documentation to convert, you'll understand how to do it.

stevemacenski gravatar imagestevemacenski ( 2017-12-21 02:48:02 -0600 )edit

I looked it but ı couldn't do this unfortunately. @smac

Mekateng gravatar imageMekateng ( 2017-12-21 04:01:31 -0600 )edit

Hello, I am having the same problem. I have a pointCloud2 message. How do I calculate the distance from the origin to a point cloud? Have you solve it? If you did, could you please share the solution? Thank you very much :)

RayROS gravatar imageRayROS ( 2019-12-30 07:52:45 -0600 )edit

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: 2017-12-20 07:21:51 -0600

Seen: 531 times

Last updated: Dec 20 '17