diff --git a/Tutorial_Lidar_Tobias.odt b/Tutorial_Lidar_Tobias.odt deleted file mode 100644 index 216478e..0000000 Binary files a/Tutorial_Lidar_Tobias.odt and /dev/null differ diff --git a/openslam_gmapping/CMakeLists.txt b/openslam_gmapping/CMakeLists.txt index b63faba..8b4f927 100644 --- a/openslam_gmapping/CMakeLists.txt +++ b/openslam_gmapping/CMakeLists.txt @@ -11,10 +11,17 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-O3 -funroll-loops) +# DEFAULT RELEASE +if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt) + if (NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE) + endif() endif() +# if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +# add_compile_options(-O3 -funroll-loops) +# endif() + add_compile_options(-fPIC) find_package(ament_cmake REQUIRED) diff --git a/openslam_gmapping/gridfastslam/gridslamprocessor.cpp b/openslam_gmapping/gridfastslam/gridslamprocessor.cpp index a4a510e..831bae9 100644 --- a/openslam_gmapping/gridfastslam/gridslamprocessor.cpp +++ b/openslam_gmapping/gridfastslam/gridslamprocessor.cpp @@ -17,7 +17,9 @@ const double m_distanceThresholdCheck = 20; using namespace std; - GridSlamProcessor::GridSlamProcessor(): m_infoStream(cout){ + + GridSlamProcessor::GridSlamProcessor() + :m_infoStream(cout){ period_ = 5.0; m_obsSigmaGain=1; diff --git a/slam_gmapping/CMakeLists.txt b/slam_gmapping/CMakeLists.txt index d0115c0..2fb6728 100644 --- a/slam_gmapping/CMakeLists.txt +++ b/slam_gmapping/CMakeLists.txt @@ -11,11 +11,17 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) endif() +# DEFAULT RELEASE +if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt) + if (NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE) + endif() +endif() + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fuse-ld=lld -Wl,--disable-new-dtags") -message(${CMAKE_SHARED_LINKER_FLAGS}) + # find dependencies find_package(ament_cmake REQUIRED) find_package(std_msgs REQUIRED) @@ -25,6 +31,7 @@ find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(message_filters REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(sensor_msgs REQUIRED) find_package(visualization_msgs REQUIRED) find_package(openslam_gmapping REQUIRED) @@ -40,24 +47,38 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -include_directories(include) +# include_directories(include) + +add_library(slam_gmapping SHARED + src/slam_gmapping.cpp) -add_executable(slam_gmapping src/slam_gmapping.cpp) +target_include_directories(slam_gmapping +PRIVATE + $ + $) ament_target_dependencies(slam_gmapping - rclcpp - tf2 - tf2_ros - message_filters - sensor_msgs - nav_msgs - tf2_geometry_msgs - openslam_gmapping) - -install(TARGETS slam_gmapping - DESTINATION lib/${PROJECT_NAME}) - -install(DIRECTORY launch + rclcpp + rclcpp_components + tf2 + tf2_ros + message_filters + sensor_msgs + nav_msgs + tf2_geometry_msgs + openslam_gmapping) + +rclcpp_components_register_node(slam_gmapping + PLUGIN "SlamGmapping" + EXECUTABLE slam_gmapping_node) + +install(TARGETS slam_gmapping + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) ament_package() \ No newline at end of file diff --git a/slam_gmapping/config/slam_gmapping.yaml b/slam_gmapping/config/slam_gmapping.yaml new file mode 100644 index 0000000..906e04f --- /dev/null +++ b/slam_gmapping/config/slam_gmapping.yaml @@ -0,0 +1,49 @@ +slam_gmapping: + ros__parameters: + base_frame: "base_link" + odom_frame: "odom" + map_frame: "map" + map_update_interval: 5.0 + + # Default: autodetect. Set maxUrange and maxRange for manual settings + # maxUrange: 10.0 + # maxRange: 10.0 + minimumScore: 0 + + sigma: 0.05 + kernelSize: 1 + lstep: 0.05 + astep: 0.05 + iterations: 5 + lsigma: 0.075 + ogain: 3.0 + lskip: 0 + + srr: 0.1 + srt: 0.2 + str: 0.1 + stt: 0.2 + + linearUpdate: 1.0 + angularUpdate: 0.5 + temporalUpdate: -1.0 + + resampleThreshold: 0.5 + particles: 30 + + # Initial Map Size & Resolution + xmin: -100.0 + ymin: -100.0 + xmax: 100.0 + ymax: 100.0 + delta: 0.05 + + llsamplerange: 0.01 + llsamplestep: 0.01 + lasamplerange: 0.005 + lasamplestep: 0.005 + + transform_publish_period: 0.05 + occ_thresh: 0.25 + + tf_delay: 0.1 \ No newline at end of file diff --git a/slam_gmapping/include/slam_gmapping/slam_gmapping.h b/slam_gmapping/include/slam_gmapping/slam_gmapping.h index 1ee2e5c..885f3fc 100644 --- a/slam_gmapping/include/slam_gmapping/slam_gmapping.h +++ b/slam_gmapping/include/slam_gmapping/slam_gmapping.h @@ -35,7 +35,7 @@ #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2_ros/transform_listener.h" #include "tf2_ros/transform_broadcaster.h" #include "tf2/utils.h" @@ -47,9 +47,9 @@ #include "gmapping/sensor/sensor_range/rangesensor.h" #include "gmapping/sensor/sensor_odometry/odometrysensor.h" -class SlamGmapping : public rclcpp::Node{ +class SlamGmapping : public rclcpp::Node { public: - SlamGmapping(); + explicit SlamGmapping(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); ~SlamGmapping() override; void init(); @@ -59,7 +59,6 @@ class SlamGmapping : public rclcpp::Node{ void publishLoop(double transform_publish_period); private: - rclcpp::Node::SharedPtr node_; rclcpp::Publisher::SharedPtr entropy_publisher_; rclcpp::Publisher::SharedPtr sst_; rclcpp::Publisher::SharedPtr sstm_; @@ -111,6 +110,7 @@ class SlamGmapping : public rclcpp::Node{ double computePoseEntropy(); // Parameters used by GMapping + bool autoRange_; double maxRange_; double maxUrange_; double maxrange_; diff --git a/slam_gmapping/launch/slam_gmapping.launch.py b/slam_gmapping/launch/slam_gmapping.launch.py index cfd68fe..7d81123 100644 --- a/slam_gmapping/launch/slam_gmapping.launch.py +++ b/slam_gmapping/launch/slam_gmapping.launch.py @@ -1,15 +1,28 @@ from launch import LaunchDescription +from ament_index_python.packages import get_package_share_directory from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +import os +from launch.actions import DeclareLaunchArgument def generate_launch_description(): - use_sim_time = LaunchConfiguration('use_sim_time', default='true') + use_sim_time = LaunchConfiguration('use_sim_time') + use_sim_time_arg = DeclareLaunchArgument( + 'use_sim_time', + default_value='false' + ) + return LaunchDescription([ + use_sim_time_arg, Node( package='slam_gmapping', - executable='slam_gmapping', + executable='slam_gmapping_node', + name='slam_gmapping', output='screen', - parameters=[{'use_sim_time': use_sim_time}] + parameters=[ + {'use_sim_time': use_sim_time}, + os.path.join(get_package_share_directory("slam_gmapping"), 'config', 'slam_gmapping.yaml') + ] ), ]) diff --git a/slam_gmapping/package.xml b/slam_gmapping/package.xml index 66e0ac7..f5e8d0d 100644 --- a/slam_gmapping/package.xml +++ b/slam_gmapping/package.xml @@ -23,6 +23,7 @@ tf2_geometry_msgs message_filters rclcpp + rclcpp_components sensor_msgs visualization_msgs openslam_gmapping diff --git a/slam_gmapping/src/slam_gmapping.cpp b/slam_gmapping/src/slam_gmapping.cpp index 8500f8e..2d97cb0 100644 --- a/slam_gmapping/src/slam_gmapping.cpp +++ b/slam_gmapping/src/slam_gmapping.cpp @@ -29,28 +29,28 @@ using std::placeholders::_1; -SlamGmapping::SlamGmapping(): - Node("slam_gmapping"), +SlamGmapping::SlamGmapping(const rclcpp::NodeOptions& options): + Node("slam_gmapping", options), scan_filter_sub_(nullptr), scan_filter_(nullptr), laser_count_(0), transform_thread_(nullptr) { buffer_ = std::make_shared(get_clock()); - auto timer_interface = std::make_shared( - get_node_base_interface(), - get_node_timers_interface()); + auto timer_interface = std::make_shared( + get_node_base_interface(), + get_node_timers_interface()); buffer_->setCreateTimerInterface(timer_interface); tfl_ = std::make_shared(*buffer_); - node_ = std::shared_ptr(this, [](rclcpp::Node *) {}); - tfB_ = std::make_shared(node_); + tfB_ = std::make_shared(this); map_to_odom_.setIdentity(); seed_ = static_cast(time(nullptr)); init(); startLiveSlam(); } -void SlamGmapping::init() { +void SlamGmapping::init() +{ gsp_ = new GMapping::GridSlamProcessor(); gsp_laser_ = nullptr; @@ -59,58 +59,67 @@ void SlamGmapping::init() { got_map_ = false; throttle_scans_ = 1; - base_frame_ = "base_link"; - map_frame_ = "map"; - odom_frame_ = "odom"; - transform_publish_period_ = 0.05; - - map_update_interval_ = tf2::durationFromSec(0.5); - maxUrange_ = 80.0; maxRange_ = 0.0; - minimum_score_ = 0; - sigma_ = 0.05; - kernelSize_ = 1; - lstep_ = 0.05; - astep_ = 0.05; - iterations_ = 5; - lsigma_ = 0.075; - ogain_ = 3.0; - lskip_ = 0; - srr_ = 0.1; - srt_ = 0.2; - str_ = 0.1; - stt_ = 0.2; - linearUpdate_ = 1.0; - angularUpdate_ = 0.5; - temporalUpdate_ = 1.0; - resampleThreshold_ = 0.5; - particles_ = 30; - xmin_ = -10.0; - ymin_ = -10.0; - xmax_ = 10.0; - ymax_ = 10.0; - delta_ = 0.05; - occ_thresh_ = 0.25; - llsamplerange_ = 0.01; - llsamplestep_ = 0.01; - lasamplerange_ = 0.005; - lasamplestep_ = 0.005; - tf_delay_ = transform_publish_period_; + + base_frame_ = declare_parameter("base_frame", "base_link"); + + map_frame_ = declare_parameter("map_frame", "map"); + odom_frame_ = declare_parameter("odom_frame", "odom"); + transform_publish_period_ = declare_parameter("transform_publish_period", 0.05); + + map_update_interval_ = tf2::durationFromSec(declare_parameter("map_update_interval", 5.0)); + + autoRange_ = declare_parameter("autoRange", true); + maxUrange_ = declare_parameter("maxUrange", 50.0); + maxRange_ = declare_parameter("maxRange", 50.0); + + minimum_score_ = declare_parameter("minimumScore", 0); + sigma_ = declare_parameter("sigma", 0.05); + kernelSize_ = declare_parameter("kernelSize", 1); + lstep_ = declare_parameter("lstep", 0.05); + astep_ = declare_parameter("astep", 0.05); + iterations_ = declare_parameter("iterations", 5); + lsigma_ = declare_parameter("lsigma", 0.075); + ogain_ = declare_parameter("ogain", 3.0); + lskip_ = declare_parameter("lskip", 0); + srr_ = declare_parameter("srr", 0.1); + srt_ = declare_parameter("srt", 0.2); + str_ = declare_parameter("str", 0.1); + stt_ = declare_parameter("stt", 0.2); + linearUpdate_ = declare_parameter("linearUpdate", 1.0); + angularUpdate_ = declare_parameter("angularUpdate", 0.5); + temporalUpdate_ = declare_parameter("temporalUpdate", 1.0); + resampleThreshold_ = declare_parameter("resampleThreshold", 0.5); + particles_ = declare_parameter("particles", 30); + xmin_ = declare_parameter("xmin", -100.0); + ymin_ = declare_parameter("ymin", -100.0); + xmax_ = declare_parameter("xmax", 100.0); + ymax_ = declare_parameter("ymax", 100.0); + delta_ = declare_parameter("delta", 0.05); + occ_thresh_ = declare_parameter("occ_thresh", 0.25); + llsamplerange_ = declare_parameter("llsamplerange", 0.01); + llsamplestep_ = declare_parameter("llsamplestep", 0.01); + lasamplerange_ = declare_parameter("lasamplerange", 0.005); + lasamplestep_ = declare_parameter("lasamplestep", 0.005); + + tf_delay_ = declare_parameter("tf_delay", transform_publish_period_); } -void SlamGmapping::startLiveSlam() { - entropy_publisher_ = this->create_publisher("entropy", rclcpp::SystemDefaultsQoS()); - sst_ = this->create_publisher("map", rclcpp::SystemDefaultsQoS()); - sstm_ = this->create_publisher("map_metadata", rclcpp::SystemDefaultsQoS()); - scan_filter_sub_ = std::make_shared> - (node_, "scan", rclcpp::SensorDataQoS().get_rmw_qos_profile()); +void SlamGmapping::startLiveSlam() +{ + entropy_publisher_ = this->create_publisher("entropy", rclcpp::SystemDefaultsQoS()); + sst_ = this->create_publisher("map", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + sstm_ = this->create_publisher("map_metadata", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + scan_filter_sub_ = std::make_shared> + (this, "scan", rclcpp::SystemDefaultsQoS().get_rmw_qos_profile()); // sub_ = this->create_subscription( // "scan", rclcpp::SensorDataQoS(), // std::bind(&SlamGmapping::laserCallback, this, std::placeholders::_1)); - scan_filter_ = std::make_shared> - (*scan_filter_sub_, *buffer_, odom_frame_, 10, node_); - scan_filter_->registerCallback(std::bind(&SlamGmapping::laserCallback, this, std::placeholders::_1)); - transform_thread_ = std::make_shared - (std::bind(&SlamGmapping::publishLoop, this, transform_publish_period_)); + scan_filter_ = std::make_shared> + (*scan_filter_sub_, *buffer_, odom_frame_, 10, + get_node_logging_interface(), get_node_clock_interface()); + scan_filter_->registerCallback(std::bind(&SlamGmapping::laserCallback, this, std::placeholders::_1)); + transform_thread_ = std::make_shared + (std::bind(&SlamGmapping::publishLoop, this, transform_publish_period_)); } void SlamGmapping::publishLoop(double transform_publish_period){ @@ -250,9 +259,13 @@ bool SlamGmapping::initMapper(const sensor_msgs::msg::LaserScan::ConstSharedPtr GMapping::OrientedPoint gmap_pose(0, 0, 0); // setting maxRange and maxUrange here so we can set a reasonable default - maxRange_ = scan->range_max - 0.01; - maxUrange_ = maxRange_; - + + if(autoRange_) + { + maxRange_ = scan->range_max - 0.01; + maxUrange_ = maxRange_; + } + // The laser must be called "FLASER". // We pass in the absolute value of the computed angle increment, on the // assumption that GMapping requires a positive angle increment. If the @@ -531,11 +544,5 @@ void SlamGmapping::publishTransform() map_to_odom_mutex_.unlock(); } -int main(int argc, char* argv[]) -{ - rclcpp::init(argc, argv); - - auto slam_gmapping_node = std::make_shared(); - rclcpp::spin(slam_gmapping_node); - return(0); -} +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(SlamGmapping) \ No newline at end of file