Skip to content

Commit

Permalink
Merge pull request #79 from UbiquityRobotics/performance
Browse files Browse the repository at this point in the history 
Cosine & Sine LookUp tables and reserve() on std::vector optimization
  • Loading branch information
@dorkamotorka
dorkamotorka authored Feb 19, 2021
2 parents b232dea + 39a25d0 commit 7c277c3
Show file tree
Hide file tree
Showing 4 changed files with 91 additions and 41 deletions.
5 changes: 2 additions & 3 deletions include/move_basic/collision_checker.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,7 @@ class CollisionChecker
float degrees(float radians) const;

public:
// Note that we take in refrences to tf_buffer and op, we expect these to outlive
// Note that we take in refrences to tf_buffer and op, we expect these to outlive
// the useful life of this class, if they don't then you have a big issue.
//
// We don't store the NodeHandle, so that doesn't apply to it.
Expand All @@ -85,12 +85,11 @@ class CollisionChecker

// return distance in radians to closest obstacle
float obstacle_angle(bool left);

float obstacle_arc_angle(double linear, double angular);

double min_side_dist;
double max_side_dist;
};

#endif

32 changes: 27 additions & 5 deletions include/move_basic/obstacle_points.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,25 @@
#include <sensor_msgs/Range.h>
#include <sensor_msgs/LaserScan.h>

// lidar sensor
class LidarSensor
{
public:
std::string frame_id;
double angle_increment;
double min_range;
double max_range;
double min_angle;
double max_angle;
LidarSensor() {};
void reset(const std::string & _frame,
const int _increment,
const double & _min_range,
const double & _max_range,
const double & _min_angle,
const double & _max_angle);
};

// a single sensor with current obstacles
class RangeSensor
{
Expand Down Expand Up @@ -81,20 +100,24 @@ class ObstaclePoints
};

std::mutex points_mutex;

std::string baseFrame;

LidarSensor lidar;
std::map<std::string, RangeSensor> sensors;
ros::Subscriber sonar_sub;
ros::Subscriber scan_sub;
tf2_ros::Buffer& tf_buffer;

bool have_lidar;
tf2::Vector3 lidar_origin;
tf2::Vector3 lidar_normal;
std::vector<PolarLine> lidar_points;
ros::Time lidar_stamp;

// Sine / Cosine LookUp Tables
std::vector<float> sinLUT, cosLUT;

// Manually added points, used for unit testing things that
// use ObstaclePoints without having to go through ROS messages
std::vector<tf2::Vector3> test_points;
Expand All @@ -111,7 +134,7 @@ class ObstaclePoints
*
*/
std::vector<tf2::Vector3> get_points(ros::Duration max_age);

/*
* Returns a vector of lines (expressed as a pair of 2 points).
* The lines are based on the end of the sonar cones, filtered
Expand All @@ -121,12 +144,11 @@ class ObstaclePoints
typedef std::pair<tf2::Vector3, tf2::Vector3> Line;
std::vector<Line> get_lines(ros::Duration max_age);

// Used for unit testing things that use ObstaclePoints
// Used for unit testing things that use ObstaclePoints
// without having to go through ROS messages
void add_test_point(tf2::Vector3 p);
void clear_test_points();

};

#endif

16 changes: 8 additions & 8 deletions src/collision_checker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,7 +72,7 @@
#include "move_basic/collision_checker.h"


CollisionChecker::CollisionChecker(ros::NodeHandle& nh, tf2_ros::Buffer &tf_buffer,
CollisionChecker::CollisionChecker(ros::NodeHandle& nh, tf2_ros::Buffer &tf_buffer,
ObstaclePoints& op) : tf_buffer(tf_buffer),
ob_points(op)
{
Expand Down Expand Up @@ -257,7 +257,7 @@ float CollisionChecker::obstacle_dist(bool forward,
fl.setY(min_dist_left);
fr.setX(robot_front_length);
fr.setY(min_dist_right);

auto pts = ob_points.get_points(ros::Duration(max_age));
for (const auto& p : pts) {
float y = p.y();
Expand Down Expand Up @@ -288,7 +288,7 @@ float CollisionChecker::obstacle_dist(bool forward,

draw_line(tf2::Vector3(robot_front_length, -min_dist_right, 0),
tf2::Vector3(robot_front_length + 2, -min_dist_right, 0), 0, 0.5, 0, 20003);

// Blue
draw_line(tf2::Vector3(robot_front_length, min_dist_left, 0),
tf2::Vector3(fl.x(), fl.y(), 0), 0, 0, 1, 30000);
Expand Down Expand Up @@ -473,12 +473,12 @@ float CollisionChecker::obstacle_arc_angle(double linear, double angular) {
const auto point_of_rotation = tf2::Vector3(0, (left) ? radius : -radius, 0);

// Critical robot corners relative to point of rotation
const auto outer_point = tf2::Vector3(-robot_back_length,
const auto outer_point = tf2::Vector3(-robot_back_length,
(left) ? -robot_width: robot_width, 0) - point_of_rotation;
const auto inner_point = tf2::Vector3(robot_front_length,
const auto inner_point = tf2::Vector3(robot_front_length,
(left) ? robot_width: -robot_width, 0) - point_of_rotation;

// Critical robot points in polar (r^2, theta) form relative to center
// Critical robot points in polar (r^2, theta) form relative to center
// of rotation
const float outer_radius_sq = outer_point.length2();
const float outer_theta = std::atan2(outer_point.y(), outer_point.x());
Expand Down Expand Up @@ -509,10 +509,10 @@ float CollisionChecker::obstacle_arc_angle(double linear, double angular) {
float closest_angle = M_PI;
const auto points = ob_points.get_points(ros::Duration(max_age));
for (const auto& p : points) {
// Trasform the obstacle point into the coordiate system with the
// Trasform the obstacle point into the coordiate system with the
// point of rotation at the origin, with the same orientation as base_link
const tf2::Vector3 p_in_rot = p - point_of_rotation;
// Radius for polar coordinates around center of rotation
// Radius for polar coordinates around center of rotation
const float p_radius_sq = p_in_rot.length2();

if(p_radius_sq < outer_radius_sq && p_radius_sq > inner_radius_sq) {
Expand Down
79 changes: 54 additions & 25 deletions src/obstacle_points.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ ObstaclePoints::ObstaclePoints(ros::NodeHandle& nh, tf2_ros::Buffer& tf_buffer)
&ObstaclePoints::range_callback, this);
scan_sub = nh.subscribe("/scan", 1,
&ObstaclePoints::scan_callback, this);

nh.param<std::string>("base_frame", baseFrame, "base_link");
}

Expand All @@ -46,7 +46,7 @@ void ObstaclePoints::range_callback(const sensor_msgs::Range::ConstPtr &msg) {
ROS_DEBUG("Callback %s %f", frame.c_str(), msg->range);

const std::lock_guard<std::mutex> lock(points_mutex);

// create sensor object if this is a new sensor
std::map<std::string,RangeSensor>::iterator it = sensors.find(frame);
if (it == sensors.end()) {
Expand Down Expand Up @@ -108,15 +108,16 @@ void ObstaclePoints::scan_callback(const sensor_msgs::LaserScan::ConstPtr &msg)
{
float theta = msg->angle_min;
float increment = msg->angle_increment;
float range_min = msg->range_min;
size_t array_size = (msg->ranges).size();

const std::lock_guard<std::mutex> lock(points_mutex);
lidar_stamp = msg->header.stamp;

if (!have_lidar) {
try {
std::string laserFrame = msg->header.frame_id;
geometry_msgs::TransformStamped laser_to_base_tf =
tf_buffer.lookupTransform(baseFrame, msg->header.frame_id, ros::Time(0));
tf_buffer.lookupTransform(baseFrame, laserFrame, ros::Time(0));

tf2::Transform tf;

Expand All @@ -138,7 +139,25 @@ void ObstaclePoints::scan_callback(const sensor_msgs::LaserScan::ConstPtr &msg)
tf2::doTransform(normal, base_normal, laser_to_base_tf);
fromMsg(base_normal.vector, lidar_normal);

double min_range = msg->range_min;
double max_range = msg->range_max;
double min_angle = msg->angle_min;
double max_angle = msg->angle_max;
double angle_increment = msg->angle_increment;
lidar.reset(laserFrame, angle_increment, min_range, max_range, min_angle, max_angle);
have_lidar = true;

// Sine / Cosine Look Up Tables
cosLUT.reserve(array_size);
sinLUT.reserve(array_size);
double angle = msg->angle_min;
for (unsigned int i = 0 ; i < array_size ; i++) {
double cosine = std::cos(angle);
double sine = std::sin(angle);
cosLUT.push_back(std::move(cosine));
sinLUT.push_back(std::move(sine));
angle += increment;
}
}
catch (tf2::TransformException &ex) {
ROS_WARN("%s", ex.what());
Expand All @@ -147,34 +166,32 @@ void ObstaclePoints::scan_callback(const sensor_msgs::LaserScan::ConstPtr &msg)
}

lidar_points.clear();
lidar_points.reserve(array_size);
for (const auto& r : msg->ranges) {
theta += increment;

// ignore bogus samples
if (std::isnan(r) || r < range_min) {
continue;
}

lidar_points.push_back(PolarLine(r, theta));
theta += increment;
}
}

std::vector<tf2::Vector3> ObstaclePoints::get_points(ros::Duration max_age) {
ros::Time now = ros::Time::now();
std::vector<tf2::Vector3> points;

const std::lock_guard<std::mutex> lock(points_mutex);
points.reserve(lidar_points.size());
ros::Duration lidar_age = now - lidar_stamp;
if (lidar_age < max_age) {
for (const auto& p : lidar_points) {
float sin_theta = std::sin(p.theta);
float cos_theta = std::cos(p.theta);
for (unsigned int i = 0 ; i < lidar_points.size() ; i++) {
float radius = lidar_points[i].radius;

// ignore bogus samples
if (std::isnan(radius) || std::isinf(radius) || radius < lidar.min_range) continue;

float x = lidar_origin.x() + p.radius * (lidar_normal.x() * cos_theta -
lidar_normal.y() * sin_theta);
float x = lidar_origin.x() + radius * (lidar_normal.x() * cosLUT[i] -
lidar_normal.y() * sinLUT[i]);

float y = lidar_origin.y() + p.radius * (lidar_normal.y() * cos_theta +
lidar_normal.x() * sin_theta);
float y = lidar_origin.y() + radius * (lidar_normal.y() * cosLUT[i] +
lidar_normal.x() * sinLUT[i]);

points.push_back(tf2::Vector3(x, y, 0));
}
Expand All @@ -198,15 +215,13 @@ std::vector<tf2::Vector3> ObstaclePoints::get_points(ros::Duration max_age) {

std::vector<ObstaclePoints::Line> ObstaclePoints::get_lines(ros::Duration max_age) {
ros::Time now = ros::Time::now();

const std::lock_guard<std::mutex> lock(points_mutex);
std::vector<ObstaclePoints::Line> lines;
for (const auto& kv : sensors) {
const RangeSensor& sensor = kv.second;
ros::Duration age = now - sensor.stamp;
if (age < max_age) {
lines.emplace_back(sensor.left_vertex, sensor.right_vertex);
}
if (age < max_age) lines.emplace_back(sensor.left_vertex, sensor.right_vertex);
}

return lines;
Expand All @@ -222,6 +237,21 @@ void ObstaclePoints::clear_test_points() {
test_points.clear();
}

void LidarSensor::reset(const std::string & _frame,
const int _increment,
const double & _min_range,
const double & _max_range,
const double & _min_angle,
const double & _max_angle)
{
this->frame_id = _frame;
this->angle_increment = _increment;
this->min_range = _min_range;
this->max_range = _max_range;
this->min_angle = _min_angle;
this->max_angle = _max_angle;
}

RangeSensor::RangeSensor(int id, std::string frame_id,
const tf2::Vector3& origin,
const tf2::Vector3& left_vec,
Expand All @@ -247,4 +277,3 @@ ObstaclePoints::PolarLine::PolarLine(float radius, float theta)
this->radius = radius;
this->theta = theta;
}

0 comments on commit 7c277c3

Please sign in to comment.