ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

When should laser scan data be converted to Point Cloud?

asked 2022-02-23 11:16:15 -0600

abhijelly gravatar image

I'm trying to digest safety_node code(below) which uses TimeToCollision(TTC) metric to enable automatic emergency braking (AEB) to prevent car from hitting obstacle more on TTC here

The node subscribes to \scan topic of LaserScan message type and converts the message type class ranges data member to a PointCloud message type. And I do not understand the purpose of doing this. Why not use the ranges data type to find TTC. Is it a standard practice to convert LaserScan to PointCloud? Why do we do this?

\scan subscriber callback code (main doubt in this part of the code)

void scan_callback(const sensor_msgs::LaserScan::ConstPtr &scan_msg) {
        double v_x = v_car - 0;
        vector<float> scan_ranges = scan_msg->ranges;
        vector<float> TTC = scan_ranges;
        float angle_increment = scan_msg->angle_increment;
        float angle_min = scan_msg->angle_min;
        float ttc_threshold = 0.32;
        // ROS_INFO_STREAM(scan_msg->header.frame_id);

        sensor_msgs::PointCloud cloud;
        tf_listener.waitForTransform(scan_msg->header.frame_id, "/center", scan_msg->header.stamp + ros::Duration().fromSec(scan_msg->ranges.size()*scan_msg->time_increment), ros::Duration(1.0));

       \\ WHY HAVE THEY DONE THIS?
        projector.transformLaserScanToPointCloud("center", *scan_msg, cloud, tf_listener, 0x02);
        vector<float> shifted_ind = cloud.channels[0].values;
        vector<float> shifted_scan_ranges = scan_ranges;

        // calculate the TTC
        for(int ind = 0; ind < shifted_ind.size(); ind++){
            // ROS_INFO_STREAM(cloud);
            double shifted_scan_range = sqrt(pow(cloud.points[ind].x, 2) + pow(cloud.points[ind].y, 2));
            shifted_scan_ranges[shifted_ind[ind]] = shifted_scan_range;
            if(!isnan(shifted_scan_range) && !isnan(shifted_scan_range)){
                // double ttc = scan_ranges[ind] / max(v_x * cos(angle_min + angle_increment * ind), 0.0);
                double ttc = shifted_scan_range / max(v_x * cos(atan2(cloud.points[ind].y, cloud.points[ind].x)), 0.0);
                if(ttc < 0){
                    TTC[ind] = numeric_limits<double>::infinity();
                }else{
                    TTC[ind] = ttc;
                }
            }else{
                TTC[ind] = numeric_limits<double>::infinity();
            }
            // ROS_INFO_STREAM(TTC[ind]);
        }

        float min_ttc = *min_element(TTC.begin(), TTC.end());
        if(min_ttc < ttc_threshold){
            Bool_msg.data = true;
        }else if(min_ttc > 1){
            Bool_msg.data = false;
        }
        if(Bool_msg.data == true){
            Bool_msg.data = true;
            ackermann_msg.drive.speed = 0.0;
            brake_bool_pub.publish(Bool_msg);
            brake_pub.publish(ackermann_msg);
        }

        if(min_ttc < 1){
            ROS_INFO_STREAM(min_ttc);
        }

    }

Entire safety_node code (note I'm not the author of this code):

#include <ros/ros.h>
#include "std_msgs/Bool.h"
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/LaserScan.h>
#include <ackermann_msgs/AckermannDriveStamped.h>
#include <tf/transform_listener.h>
#include <laser_geometry/laser_geometry.h>
#include <sensor_msgs/PointCloud.h>
#include <geometry_msgs/Point32.h>

#include <vector>
#include <algorithm>
#include <cmath>
// #include <iostream> 
using namespace std;

// Solution based on submission of Zirui Zang

class Safety {
// The class that handles emergency braking
private:
    ros::NodeHandle n;
    double v_car;

    ros::Subscriber laser_sub;
    ros::Subscriber odom_sub;
    ros::Publisher brake_pub;
    ros::Publisher brake_bool_pub;
    laser_geometry::LaserProjection projector;
    tf::TransformListener tf_listener;

    std_msgs::Bool Bool_msg;
    // Bool_msg.data = false;
    ackermann_msgs::AckermannDriveStamped ackermann_msg;

public:

    Safety() {
        n = ros::NodeHandle();
        v_car = 0.0;
        /*
        One publisher should publish to the /brake topic with an
        ackermann_msgs/AckermannDriveStamped brake message.

        One publisher should publish to the /brake_bool topic with a
        std_msgs/Bool message.

        You should also subscribe to the /scan topic to get the
        sensor_msgs/LaserScan messages and the /odom topic to get
        the nav_msgs/Odometry messages

        The subscribers should use the provided odom_callback and 
        scan_callback as callback methods

        NOTE that the x component of the linear velocity in odom is the speed
        */

        laser_sub = n.subscribe ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2022-02-23 13:40:05 -0600

Most probably because it handles the conversion into euclidean space that can be more easily used to represent collision boxes or other non-radial-range defined shapes. However, from glancing at the code, I fully agree with you, for what they're doing, there's no reason I can discern for it. They're backing out the range from X and Y components and backing out the angle; both were simply given in the laser scan message.

edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2022-02-23 11:16:15 -0600

Seen: 168 times

Last updated: Feb 23 '22