diff --git a/.gitignore b/.gitignore index e4cb27f..e90b4ec 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,395 @@ +# Created by https://www.toptal.com/developers/gitignore/api/visualstudiocode,vim,python,c++,sublimetext,windows,macos,ros +# Edit at https://www.toptal.com/developers/gitignore?templates=visualstudiocode,vim,python,c++,sublimetext,windows,macos,ros + +### C++ ### +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### macOS ### +# General +.DS_Store +.AppleDouble +.LSOverride + +# Icon must end with two \r +Icon + + +# Thumbnails +._* + +# Files that might appear in the root of a volume +.DocumentRevisions-V100 +.fseventsd +.Spotlight-V100 +.TemporaryItems +.Trashes +.VolumeIcon.icns +.com.apple.timemachine.donotpresent + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + +### macOS Patch ### +# iCloud generated files +*.icloud + +### Python ### +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +share/python-wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.nox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +*.py,cover +.hypothesis/ +.pytest_cache/ +cover/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py +db.sqlite3 +db.sqlite3-journal + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +.pybuilder/ +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# IPython +profile_default/ +ipython_config.py + +# pyenv +# For a library or package, you might want to ignore these files since the code is +# intended to run in multiple environments; otherwise, check them in: +# .python-version + +# pipenv +# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. +# However, in case of collaboration, if having platform-specific dependencies or dependencies +# having no cross-platform support, pipenv may install dependencies that don't work, or not +# install all needed dependencies. +#Pipfile.lock + +# poetry +# Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. +# This is especially recommended for binary packages to ensure reproducibility, and is more +# commonly ignored for libraries. +# https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control +#poetry.lock + +# pdm +# Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. +#pdm.lock +# pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it +# in version control. +# https://pdm.fming.dev/#use-with-ide +.pdm.toml + +# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm +__pypackages__/ + +# Celery stuff +celerybeat-schedule +celerybeat.pid + +# SageMath parsed files +*.sage.py + +# Environments +.env +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ +.dmypy.json +dmypy.json + +# Pyre type checker +.pyre/ + +# pytype static type analyzer +.pytype/ + +# Cython debug symbols +cython_debug/ + +# PyCharm +# JetBrains specific template is maintained in a separate JetBrains.gitignore that can +# be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore +# and can be added to the global gitignore or merged into this file. For a more nuclear +# option (not recommended) you can uncomment the following to ignore the entire idea folder. +#.idea/ + +### Python Patch ### +# Poetry local configuration file - https://python-poetry.org/docs/configuration/#local-configuration +poetry.toml + +# ruff +.ruff_cache/ + +# LSP config files +pyrightconfig.json + +### ROS ### +devel/ +logs/ +bin/ +msg_gen/ +srv_gen/ +msg/*Action.msg +msg/*ActionFeedback.msg +msg/*ActionGoal.msg +msg/*ActionResult.msg +msg/*Feedback.msg +msg/*Goal.msg +msg/*Result.msg +msg/_*.py +build_isolated/ +devel_isolated/ + +# Generated by dynamic reconfigure +*.cfgc +/cfg/cpp/ +/cfg/*.py + +# Ignore generated docs +*.dox +*.wikidoc + +# eclipse stuff +.project +.cproject + +# qcreator stuff +CMakeLists.txt.user + +srv/_*.py +*.pcd +*.pyc +qtcreator-* +*.user + +/planning/cfg +/planning/docs +/planning/src + +*~ + +# Emacs +.#* + +# Catkin custom files +CATKIN_IGNORE + +### SublimeText ### +# Cache files for Sublime Text +*.tmlanguage.cache +*.tmPreferences.cache +*.stTheme.cache + +# Workspace files are user-specific +*.sublime-workspace + +# Project files should be checked into the repository, unless a significant +# proportion of contributors will probably not be using Sublime Text +# *.sublime-project + +# SFTP configuration file +sftp-config.json +sftp-config-alt*.json + +# Package control specific files +Package Control.last-run +Package Control.ca-list +Package Control.ca-bundle +Package Control.system-ca-bundle +Package Control.cache/ +Package Control.ca-certs/ +Package Control.merged-ca-bundle +Package Control.user-ca-bundle +oscrypto-ca-bundle.crt +bh_unicode_properties.cache + +# Sublime-github package stores a github token in this file +# https://packagecontrol.io/packages/sublime-github +GitHub.sublime-settings + +### Vim ### +# Swap +[._]*.s[a-v][a-z] +!*.svg # comment out if you don't need vector files +[._]*.sw[a-p] +[._]s[a-rt-v][a-z] +[._]ss[a-gi-z] +[._]sw[a-p] + +# Session +Session.vim +Sessionx.vim + +# Temporary +.netrwhist +# Auto-generated tag files +tags +# Persistent undo +[._]*.un~ + +### VisualStudioCode ### +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json +!.vscode/*.code-snippets + +# Local History for Visual Studio Code +.history/ + +# Built Visual Studio Code Extensions +*.vsix + +### VisualStudioCode Patch ### +# Ignore all local history of files +.history +.ionide + +### Windows ### +# Windows thumbnail cache files +Thumbs.db +Thumbs.db:encryptable +ehthumbs.db +ehthumbs_vista.db + +# Dump file +*.stackdump + +# Folder config file +[Dd]esktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Windows Installer files +*.cab +*.msi +*.msix +*.msm +*.msp + +# Windows shortcuts +*.lnk + +# End of https://www.toptal.com/developers/gitignore/api/visualstudiocode,vim,python,c++,sublimetext,windows,macos,ros + build lib bin diff --git a/CMakeLists.txt b/CMakeLists.txt index bacb688..235fa1a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,7 +6,7 @@ MESSAGE(STATUS "Using compiler: ${CMAKE_CXX_COMPILER}") MESSAGE(STATUS "Build Type: ${CMAKE_BUILD_TYPE}") MESSAGE(STATUS "Arch: ${CMAKE_SYSTEM_PROCESSOR}") -SET(CMAKE_CXX_FLAGS "-std=c++14 -Wall -Werror") +SET(CMAKE_CXX_FLAGS "-std=c++17 -Wall -Werror") SET(LIBRARY_NAME "graph_nav_lib" CACHE STRING "Name of compiled library") diff --git a/config/navigation.lua b/config/navigation.lua index 127fb10..e75af62 100644 --- a/config/navigation.lua +++ b/config/navigation.lua @@ -3,16 +3,16 @@ function deg2rad(deg) end NavigationParameters = { - laser_topic = "/velodyne_2dscan"; + laser_topics = { + "/velodyne_2dscan", + "/kinect_laserscan", + }; + laser_frame = "base_link"; odom_topic = "/jackal_velocity_controller/odom"; localization_topic = "localization"; image_topic = "/camera/rgb/image_raw/compressed"; init_topic = "initialpose"; enable_topic = "autonomy_arbiter/enabled"; - laser_loc = { - x = 0.065; - y = 0; - }; dt = 0.025; max_linear_accel = 0.5; max_linear_decel = 0.5; diff --git a/src/navigation/navigation_main.cc b/src/navigation/navigation_main.cc index 95ed9a3..c1c10c1 100644 --- a/src/navigation/navigation_main.cc +++ b/src/navigation/navigation_main.cc @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -67,6 +68,7 @@ #include "std_msgs/Bool.h" #include "tf/transform_broadcaster.h" #include "tf/transform_datatypes.h" +#include "tf/transform_listener.h" #include "visualization/visualization.h" #include "motion_primitives.h" @@ -88,8 +90,10 @@ using ros_helpers::RosPoint; using ros_helpers::SetRosVector; using std::string; using std::vector; +using std::unordered_map; using sensor_msgs::PointCloud; using Eigen::Vector2f; +using Eigen::Vector3f; using graph_navigation::graphNavSrv; using geometry_msgs::TwistStamped; using geometry::kEpsilon; @@ -104,13 +108,12 @@ DEFINE_string(maps_dir, kAmrlMapsDir, "Directory containing AMRL maps"); DEFINE_bool(no_joystick, true, "Whether to use a joystick or not"); CONFIG_STRING(image_topic, "NavigationParameters.image_topic"); -CONFIG_STRING(laser_topic, "NavigationParameters.laser_topic"); +CONFIG_STRINGLIST(laser_topics, "NavigationParameters.laser_topics"); +CONFIG_STRING(laser_frame, "NavigationParameters.laser_frame"); CONFIG_STRING(odom_topic, "NavigationParameters.odom_topic"); CONFIG_STRING(localization_topic, "NavigationParameters.localization_topic"); CONFIG_STRING(init_topic, "NavigationParameters.init_topic"); CONFIG_STRING(enable_topic, "NavigationParameters.enable_topic"); -CONFIG_FLOAT(laser_loc_x, "NavigationParameters.laser_loc.x"); -CONFIG_FLOAT(laser_loc_y, "NavigationParameters.laser_loc.y"); DEFINE_string(map, "UT_Campus", "Name of navigation map file"); DEFINE_string(twist_drive_topic, "navigation/cmd_vel", "Drive Command Topic"); @@ -134,6 +137,13 @@ sensor_msgs::LaserScan last_laser_msg_; cv::Mat last_image_; Navigation navigation_; +struct LaserCache { + double time = 0.0; + float dtheta = 0.0f; + float angle_min = 0.0f; + vector rays; +}; + // Publishers ros::Publisher ackermann_drive_pub_; ros::Publisher vis_pub_; @@ -180,44 +190,78 @@ void OdometryCallback(const nav_msgs::Odometry& msg) { navigation_.UpdateOdometry(odom_); } -void LaserHandler(const sensor_msgs::LaserScan& msg) { +void RetrieveTransform(const std_msgs::Header& msg, + Eigen::Affine3f& frame_tf) { + static tf::TransformListener tf_listener; + tf::StampedTransform transform; + try { + tf_listener.waitForTransform(CONFIG_laser_frame, + msg.frame_id, + msg.stamp, + ros::Duration(0.1)); + tf_listener.lookupTransform(CONFIG_laser_frame, + msg.frame_id, + msg.stamp, + transform); + frame_tf = Eigen::Translation3f(transform.getOrigin().getX(), + transform.getOrigin().getY(), + transform.getOrigin().getZ()) * + Eigen::Quaternionf(transform.getRotation().getW(), + transform.getRotation().getX(), + transform.getRotation().getY(), + transform.getRotation().getZ()); + } catch (tf::TransformException& ex) { + ROS_ERROR("%s", ex.what()); + } +} + +void LaserHandler(const sensor_msgs::LaserScan& msg, + const string& topic) { if (FLAGS_v > 2) { - printf("Laser t=%f, dt=%f\n", + printf("Laser topic=%s, t=%f, dt=%f\n", + topic.c_str(), msg.header.stamp.toSec(), GetWallTime() - msg.header.stamp.toSec()); } - // Location of the laser on the robot. Assumes the laser is forward-facing. - const Vector2f kLaserLoc(CONFIG_laser_loc_x, CONFIG_laser_loc_x); - static float cached_dtheta_ = 0; - static float cached_angle_min_ = 0; - static size_t cached_num_rays_ = 0; - static vector cached_rays_; - if (cached_angle_min_ != msg.angle_min || - cached_dtheta_ != msg.angle_increment || - cached_num_rays_ != msg.ranges.size()) { - cached_angle_min_ = msg.angle_min; - cached_dtheta_ = msg.angle_increment; - cached_num_rays_ = msg.ranges.size(); - cached_rays_.resize(cached_num_rays_); - for (size_t i = 0; i < cached_num_rays_; ++i) { - const float a = - cached_angle_min_ + static_cast(i) * cached_dtheta_; - cached_rays_[i] = Vector2f(cos(a), sin(a)); + + static unordered_map laser_caches_; + laser_caches_.emplace(topic, LaserCache()); + LaserCache& cache = laser_caches_[topic]; + + if (cache.dtheta != msg.angle_increment || + cache.angle_min != msg.angle_min || + cache.rays.size() != msg.ranges.size()) { + cache.dtheta = msg.angle_increment; + cache.angle_min = msg.angle_min; + cache.rays.resize(msg.ranges.size()); + for (size_t i = 0; i < cache.rays.size(); ++i) { + const float a = cache.angle_min + static_cast(i) * cache.dtheta; + cache.rays[i] = Vector3f(cos(a), sin(a), 0.0f); } } - CHECK_EQ(cached_rays_.size(), msg.ranges.size()); - point_cloud_.resize(cached_rays_.size()); - for (size_t i = 0; i < cached_num_rays_; ++i) { + CHECK_EQ(cache.rays.size(), msg.ranges.size()); + + // Lookup tf transform from laser frame to base frame. + Eigen::Affine3f frame_tf; + RetrieveTransform(msg.header, frame_tf); + + size_t start_idx = point_cloud_.size(); + point_cloud_.resize(start_idx + cache.rays.size()); + for (size_t i = 0; i < cache.rays.size(); ++i) { const float r = ((msg.ranges[i] > msg.range_min && msg.ranges[i] < msg.range_max) ? msg.ranges[i] : msg.range_max); - point_cloud_[i] = r * cached_rays_[i] + kLaserLoc; - } + point_cloud_[start_idx + i] = (frame_tf * (r * cache.rays[i])).head<2>(); + } } -void LaserCallback(const sensor_msgs::LaserScan& msg) { - received_laser_ = true; - LaserHandler(msg); +void LaserCallback(const sensor_msgs::LaserScan& msg, + const string& topic) { + if (!received_laser_) { + point_cloud_.clear(); + received_laser_ = true; + } + LaserHandler(msg, topic); navigation_.ObservePointCloud(point_cloud_, msg.header.stamp.toSec()); } @@ -238,7 +282,7 @@ void GoToCallbackAMRL(const amrl_msgs::Localization2DMsg& msg) { } bool PlanServiceCb(graphNavSrv::Request &req, - graphNavSrv::Response &res) { + graphNavSrv::Response &res) { const Vector2f start(req.start.x, req.start.y); const Vector2f end(req.end.x, req.end.y); const vector plan = navigation_.GlobalPlan(start, end); @@ -830,8 +874,14 @@ int main(int argc, char** argv) { n.subscribe(CONFIG_odom_topic, 1, &OdometryCallback); ros::Subscriber localization_sub = n.subscribe(CONFIG_localization_topic, 1, &LocalizationCallback); - ros::Subscriber laser_sub = - n.subscribe(CONFIG_laser_topic, 1, &LaserCallback); + vector laser_subs(CONFIG_laser_topics.size()); + for (size_t i = 0; i < CONFIG_laser_topics.size(); ++i) { + laser_subs[i] = n.subscribe( + CONFIG_laser_topics[i], 1, + [i](const sensor_msgs::LaserScan::ConstPtr& msg_ptr) { + LaserCallback(*msg_ptr, CONFIG_laser_topics[i]); + }); + } ros::Subscriber img_sub = n.subscribe(CONFIG_image_topic, 1, &ImageCallback); ros::Subscriber goto_sub = @@ -856,6 +906,7 @@ int main(int argc, char** argv) { while (run_ && ros::ok()) { visualization::ClearVisualizationMsg(local_viz_msg_); visualization::ClearVisualizationMsg(global_viz_msg_); + received_laser_ = false; ros::spinOnce(); // Run Navigation to get commands