0
$\begingroup$

Rosanswers logo

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()

Originally posted by Mekateng on ROS Answers with karma: 85 on 2017-12-20

Post score: 0


Original comments

Comment by Mekateng on 2017-12-20:
Do you have any idea Mr. @bvbdort ?

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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


Originally posted by stevemacenski with karma: 8272 on 2017-12-20

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by Mekateng on 2017-12-21:
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.

Comment by stevemacenski on 2017-12-21:
I trust that once you look at the ros_pcl documentation to convert, you'll understand how to do it.

Comment by Mekateng on 2017-12-21:
I looked it but ı couldn't do this unfortunately. @smac

Comment by RayROS on 2019-12-30:
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 :)

Comment by Delbina on 2022-08-05:
@Mekateng gravatar would you please share the correct code?

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.