Improve Lidar-based obstacle detection #30

jim-v · 2018-01-28T09:50:37Z

Moves handling of LaserScan messages into ObstacleDetector class #28

rohbotics · 2018-01-28T15:03:41Z

src/obstacle_detector.cpp

@@ -217,6 +289,25 @@ void ObstacleDetector::get_points()
    obstacle_mutex.unlock();
 }

+// Get points from lidar - convert from polar to cartesian coords
+void ObstacleDetector::get_lidar_points(std::vector<tf2::Vector3>& points)


It doesn't 'get', since it doesn't return. Call it 'add_lidar_points'

Actually it may be worth keeping it get so it is consistent in naming with the range senors

rohbotics · 2018-01-28T15:09:31Z

include/move_basic/obstacle_detector.h


 // Handle Range messages and computes distance to obstacles
 class ObstacleDetector
 {
   std::map<std::string, RangeSensor> sensors;
   ros::Subscriber sonar_sub;
+   ros::Subscriber scan_sub;;


Double semicolon

rohbotics · 2018-01-28T15:10:41Z

include/move_basic/obstacle_detector.h

+public:
+    float radius;
+    float theta;
+


This should also take the origin of the line, and maybe the normal vector.

See comment around L300 for why.

rohbotics · 2018-01-28T15:20:09Z

src/obstacle_detector.cpp

+        float y = lidar_origin.y() + p.radius * (lidar_normal.y() * cos_theta +
+             lidar_normal.x() * sin_theta);
+
+        points.push_back(tf2::Vector3(x, y, 0));


I think all of the code to compute this vector should be within PolarLine, in a function that returns a Vector3. (Which would mean that PolarLine would need the lidar origin and normal).

At least going from polar to Cartesian should be, and those points could be transformed to the robot coordinate system here (still a function that returns a vector3).

The points are transformed to a 2D projection of base_link here, that's what lidar_origin and lidar_normal do.

I'm not sure if it makes much sense to have a class that has a point in r, theta form and has its origin in point normal form. I think it's cleaner if the coordinate system transform is done outside the class.

Ok, that makes sense.

rohbotics · 2018-01-28T15:22:18Z

src/obstacle_detector.cpp

+    if (!have_lidar) {
+        try {
+            geometry_msgs::TransformStamped tfs =
+                tf_buffer->lookupTransform("base_link", "laser", ros::Time(0));


A more sensical name for the variable like laser_tf or base2laser_tf would make the following code clearer

rohbotics · 2018-01-28T15:23:21Z

src/obstacle_detector.cpp

+            normal.vector.z = 0.0;
+            geometry_msgs::Vector3Stamped base_normal;
+            tf2::doTransform(normal, base_normal, tfs);
+            fromMsg(base_normal.vector, lidar_normal);


More of a question:
Why do we need the normal?

I see it is being used in the Polar to xy computation, but I don't understand why

It allows us to have the lidar in any orientation, instead of assuming that it is pointing forward, as the previous code did.

rohbotics · 2018-01-28T15:32:49Z

src/obstacle_detector.cpp

@@ -413,3 +555,10 @@ void RangeSensor::update(float range, ros::Time stamp)
    left_vertex = origin + left_vec * range;
    right_vertex = origin + right_vec * range;
 }
+
+PolarLine::PolarLine(float radius, float theta)


Just put this inline the class.

rohbotics · 2018-01-28T15:44:00Z

src/obstacle_detector.cpp

@@ -378,6 +482,44 @@ float ObstacleDetector::obstacle_angle(bool left)
           }
        }
    }
+
+    // Draw rotated footprint to show limit of rotation


Does it really make sense to draw the whole footprint?

I think we should be only drawing the side of the box in the direction of the turn. So if turning left, only show the left side of the box.

When turning, an obstacle collision could be with any of the four sides of the robot.

Here's an obstacle in front

rohbotics · 2018-01-28T15:45:17Z

src/obstacle_detector.cpp

+            check_dist(p.x(), forward, min_dist);
+        }
+    }
+
    if (forward) {
        draw_line(tf2::Vector3(min_dist, -robot_width, 0),
                  tf2::Vector3(min_dist, robot_width, 0), 1, 0, 0, 1000);


Shouldn't forward and backward have the same line id so that they replace each other. Otherwise we would have old data on screen until the robot moves in that direction.

Yes, good point.

rohbotics · 2018-01-28T16:58:40Z

Just curious, how well does this work for detecting/stopping for chair legs?

jim-v · 2018-01-28T19:18:40Z

Coffee table

…everse lines

rohbotics · 2018-01-28T23:44:21Z

src/obstacle_detector.cpp

@@ -158,14 +165,70 @@ void ObstacleDetector::sensor_callback(const sensor_msgs::Range::ConstPtr &msg)
    else {
        RangeSensor& sensor = sensors[frame];


Another minor comment, not necessarily part of this PR:

We are effectively running find twice because we are not using the value of the iterator it here.

I think this line could become RangeSensor& sensor = *it; which means we are only doing the map lookup once.

jim-v · 2018-01-29T02:57:51Z

@rohbotics thanks for the review. I think it's ready to merge now, do you agree?

rohbotics

👍 Looks good now

jim-v added 2 commits January 27, 2018 15:39

Changed colors

baf3b3a

Moved support for lidar to ObstacleDetector class

6800dfb

ghost assigned jim-v Jan 28, 2018

ghost added the in progress label Jan 28, 2018

jim-v changed the title ~~Obstacles~~ Improve Lidar-based obstacle detection Jan 28, 2018

jim-v requested a review from rohbotics January 28, 2018 09:56

jim-v added in review and removed in progress labels Jan 28, 2018

Draw rotation limit box in correct orientation when turning right

1fb47c9

rohbotics requested changes Jan 28, 2018

View reviewed changes

Move PolarLine inside ObstacleDetector, use same id for forward and r…

ec13fdf

…everse lines

rohbotics reviewed Jan 28, 2018

View reviewed changes

Renamed some variables for clariy, using iterator more efficiently

f147018

rohbotics approved these changes Jan 29, 2018

View reviewed changes

jim-v merged commit e9624a2 into kinetic-devel Jan 29, 2018

ghost removed the in review label Jan 29, 2018

jim-v deleted the obstacles branch January 29, 2018 03:29

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Improve Lidar-based obstacle detection #30

Improve Lidar-based obstacle detection #30

jim-v commented Jan 28, 2018 •

edited

Loading

rohbotics Jan 28, 2018

rohbotics Jan 28, 2018

rohbotics Jan 28, 2018

rohbotics Jan 28, 2018

rohbotics Jan 28, 2018

rohbotics Jan 28, 2018

jim-v Jan 28, 2018 •

edited

Loading

jim-v Jan 28, 2018

rohbotics Jan 28, 2018

rohbotics Jan 28, 2018

rohbotics Jan 28, 2018

jim-v Jan 28, 2018

rohbotics Jan 28, 2018

rohbotics Jan 28, 2018

jim-v Jan 28, 2018

jim-v Jan 28, 2018

rohbotics Jan 28, 2018

jim-v Jan 28, 2018

rohbotics commented Jan 28, 2018

jim-v commented Jan 28, 2018

rohbotics Jan 28, 2018

jim-v Jan 28, 2018

jim-v commented Jan 29, 2018

rohbotics left a comment

		@@ -158,14 +165,70 @@ void ObstacleDetector::sensor_callback(const sensor_msgs::Range::ConstPtr &msg)
		else {
		RangeSensor& sensor = sensors[frame];

Improve Lidar-based obstacle detection #30

Improve Lidar-based obstacle detection #30

Conversation

jim-v commented Jan 28, 2018 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

jim-v Jan 28, 2018 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

rohbotics commented Jan 28, 2018

jim-v commented Jan 28, 2018

Choose a reason for hiding this comment

Choose a reason for hiding this comment

jim-v commented Jan 29, 2018

rohbotics left a comment

Choose a reason for hiding this comment

jim-v commented Jan 28, 2018 •

edited

Loading

jim-v Jan 28, 2018 •

edited

Loading