diff --git a/README.md b/README.md index c3babdb..f76a287 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,7 @@ SLAM(Simultaneous Localization and Mapping) is the computational problem of constructing or updating a map of an unknown environment while simultaneously keeping track of an agent's location within it. -This contains package ```openslam_gmapping``` and ```slam_gmapping``` which is a ROS2 wrapper for OpenSlam's Gmapping. Using slam_gmapping, you can create a 2-D occupancy grid map (like a building floorplan) from laser and pose data collected by a mobile robot. +This contains package ``openslam_gmapping`` and ``slam_gmapping`` which is a ROS2 wrapper for OpenSlam's Gmapping. Using slam_gmapping, you can create a 2-D occupancy grid map (like a building floorplan) from laser and pose data collected by a mobile robot. ## Launch: @@ -12,6 +12,38 @@ ros2 launch slam_gmapping slam_gmapping.launch.py The node slam_gmapping subscribes to sensor_msgs/LaserScan on ros2 topic ``scan``. It also expects appropriate TF to be available. -It publishes the nav_msgs/OccupancyGrid on ``map``. +It publishes the nav_msgs/OccupancyGrid on ``map``. Map Meta Data and Entropy is published on ``map_metadata`` and ``entropy`` respectively. + + +## Test with turtlebot3: + +#### Install Nav2 packages + +```bash +sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup +sudo apt install ros-humble-turtlebot3* +``` + + + +#### Launch + +``` +ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py +ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True +``` + +``` +ros2 launch slam_gmapping slam_gmapping.launch.py + +ros2 run rviz2 rviz2 -d ~/slam_gmapping/gmapping.rviz +``` + +you can use explore lite ore teleop + +``` +ros2 run turtlebot3_teleop teleop_keyboard +``` +![dynamic transform](slam_gmapping/media/gmapping_test.gif) diff --git a/gmapping.rviz b/gmapping.rviz new file mode 100644 index 0000000..38ff6cf --- /dev/null +++ b/gmapping.rviz @@ -0,0 +1,163 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 617 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: map_updates + Use Timestamp: false + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap_updates + Use Timestamp: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 15.735194206237793 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.3253974914550781 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.025398634374141693 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001b5000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000001e0000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 3321 + Y: 113 diff --git a/slam_gmapping/include/slam_gmapping/slam_gmapping.h b/slam_gmapping/include/slam_gmapping/slam_gmapping.h index 1ee2e5c..1a66d0e 100644 --- a/slam_gmapping/include/slam_gmapping/slam_gmapping.h +++ b/slam_gmapping/include/slam_gmapping/slam_gmapping.h @@ -88,7 +88,7 @@ class SlamGmapping : public rclcpp::Node{ bool got_map_; nav_msgs::msg::OccupancyGrid map_; - + double map_update_interval_double_; //to recieve double from yaml file tf2::Duration map_update_interval_; tf2::Transform map_to_odom_; std::mutex map_to_odom_mutex_; diff --git a/slam_gmapping/launch/slam_gmapping.launch.py b/slam_gmapping/launch/slam_gmapping.launch.py index 0f99610..a3a980c 100644 --- a/slam_gmapping/launch/slam_gmapping.launch.py +++ b/slam_gmapping/launch/slam_gmapping.launch.py @@ -1,12 +1,28 @@ from launch import LaunchDescription -from launch.substitutions import EnvironmentVariable -import launch.actions import launch_ros.actions - +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument def generate_launch_description(): - use_sim_time = launch.substitutions.LaunchConfiguration('use_sim_time', default='true') + use_sim_time = LaunchConfiguration('use_sim_time', default='true') + param_file = LaunchConfiguration('param_file', default='slam_gmapping/param/slam_gmapping_params.yaml') + return LaunchDescription([ + DeclareLaunchArgument( + 'param_file', + default_value='slam_gmapping/param/slam_gmapping_params.yaml', + description='Path to the YAML file with parameters relative to the package' + ), + DeclareLaunchArgument( + 'use_sim_time', + default_value='true', + description='Use simulation time if true' + ), + launch_ros.actions.Node( - package='slam_gmapping', node_executable='slam_gmapping', output='screen', parameters=[{'use_sim_time':use_sim_time}]), + package='slam_gmapping', + executable='slam_gmapping', + output='screen', + parameters=[param_file, {'use_sim_time': use_sim_time}], + ), ]) diff --git a/slam_gmapping/media/gmapping_test.gif b/slam_gmapping/media/gmapping_test.gif new file mode 100644 index 0000000..66cbfe5 Binary files /dev/null and b/slam_gmapping/media/gmapping_test.gif differ diff --git a/slam_gmapping/param/slam_gmapping_params.yaml b/slam_gmapping/param/slam_gmapping_params.yaml new file mode 100644 index 0000000..0a97a62 --- /dev/null +++ b/slam_gmapping/param/slam_gmapping_params.yaml @@ -0,0 +1,39 @@ +slam_gmapping: + ros__parameters: + throttle_scans: 1 + base_frame: "base_link" + map_frame: "map" + odom_frame: "odom" + transform_publish_period: 0.05 + map_update_interval: 0.5 + maxUrange: 80.0 + maxRange: 0.0 + minimum_score: 0.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: 0.05 diff --git a/slam_gmapping/src/slam_gmapping.cpp b/slam_gmapping/src/slam_gmapping.cpp index 8500f8e..5df0705 100644 --- a/slam_gmapping/src/slam_gmapping.cpp +++ b/slam_gmapping/src/slam_gmapping.cpp @@ -31,13 +31,13 @@ using std::placeholders::_1; SlamGmapping::SlamGmapping(): Node("slam_gmapping"), - scan_filter_sub_(nullptr), - scan_filter_(nullptr), - laser_count_(0), - transform_thread_(nullptr) + 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( + auto timer_interface = std::make_shared( get_node_base_interface(), get_node_timers_interface()); buffer_->setCreateTimerInterface(timer_interface); @@ -46,6 +46,44 @@ SlamGmapping::SlamGmapping(): tfB_ = std::make_shared(node_); map_to_odom_.setIdentity(); seed_ = static_cast(time(nullptr)); + // Declare parameters with their default values + this->declare_parameter("throttle_scans", 1); + this->declare_parameter("base_frame", "base_link"); + this->declare_parameter("map_frame", "map"); + this->declare_parameter("odom_frame", "odom"); + this->declare_parameter("transform_publish_period", 0.05); + this->declare_parameter("map_update_interval", 0.5); + this->declare_parameter("maxUrange", 80.0); + this->declare_parameter("maxRange", 0.0); + this->declare_parameter("minimum_score", 0.0); + this->declare_parameter("sigma", 0.05); + this->declare_parameter("kernelSize", 1); + this->declare_parameter("lstep", 0.05); + this->declare_parameter("astep", 0.05); + this->declare_parameter("iterations", 5); + this->declare_parameter("lsigma", 0.075); + this->declare_parameter("ogain", 3.0); + this->declare_parameter("lskip", 0); + this->declare_parameter("srr", 0.1); + this->declare_parameter("srt", 0.2); + this->declare_parameter("str", 0.1); + this->declare_parameter("stt", 0.2); + this->declare_parameter("linearUpdate", 1.0); + this->declare_parameter("angularUpdate", 0.5); + this->declare_parameter("temporalUpdate", 1.0); + this->declare_parameter("resampleThreshold", 0.5); + this->declare_parameter("particles", 30); + this->declare_parameter("xmin", -10.0); + this->declare_parameter("ymin", -10.0); + this->declare_parameter("xmax", 10.0); + this->declare_parameter("ymax", 10.0); + this->declare_parameter("delta", 0.05); + this->declare_parameter("occ_thresh", 0.25); + this->declare_parameter("llsamplerange", 0.01); + this->declare_parameter("llsamplestep", 0.01); + this->declare_parameter("lasamplerange", 0.005); + this->declare_parameter("lasamplestep", 0.005); + this->declare_parameter("tf_delay", 0.05); init(); startLiveSlam(); } @@ -58,42 +96,50 @@ void SlamGmapping::init() { got_first_scan_ = false; 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; + // Retrieve parameters from the ROS parameter server + this->get_parameter("throttle_scans", throttle_scans_); + this->get_parameter("base_frame", base_frame_); + this->get_parameter("map_frame", map_frame_); + this->get_parameter("odom_frame", odom_frame_); + this->get_parameter("transform_publish_period", transform_publish_period_); + + this->get_parameter("map_update_interval", map_update_interval_double_); + + // Convert double to tf2::Duration + map_update_interval_ = tf2::durationFromSec(map_update_interval_double_); + + this->get_parameter("maxUrange", maxUrange_); + this->get_parameter("maxRange", maxRange_); + this->get_parameter("minimum_score", minimum_score_); + this->get_parameter("sigma", sigma_); + this->get_parameter("kernelSize", kernelSize_); + this->get_parameter("lstep", lstep_); + this->get_parameter("astep", astep_); + this->get_parameter("iterations", iterations_); + this->get_parameter("lsigma", lsigma_); + this->get_parameter("ogain", ogain_); + this->get_parameter("lskip", lskip_); + this->get_parameter("srr", srr_); + this->get_parameter("srt", srt_); + this->get_parameter("str", str_); + this->get_parameter("stt", stt_); + this->get_parameter("linearUpdate", linearUpdate_); + this->get_parameter("angularUpdate", angularUpdate_); + this->get_parameter("temporalUpdate", temporalUpdate_); + this->get_parameter("resampleThreshold", resampleThreshold_); + this->get_parameter("particles", particles_); + this->get_parameter("xmin", xmin_); + this->get_parameter("ymin", ymin_); + this->get_parameter("xmax", xmax_); + this->get_parameter("ymax", ymax_); + this->get_parameter("delta", delta_); + this->get_parameter("occ_thresh", occ_thresh_); + this->get_parameter("llsamplerange", llsamplerange_); + this->get_parameter("llsamplestep", llsamplestep_); + this->get_parameter("lasamplerange", lasamplerange_); + this->get_parameter("lasamplestep", lasamplestep_); + + // Ensure tf_delay is set to the value of transform_publish_period_ tf_delay_ = transform_publish_period_; } @@ -103,9 +149,9 @@ void SlamGmapping::startLiveSlam() { sstm_ = this->create_publisher("map_metadata", rclcpp::SystemDefaultsQoS()); scan_filter_sub_ = std::make_shared> (node_, "scan", rclcpp::SensorDataQoS().get_rmw_qos_profile()); -// sub_ = this->create_subscription( -// "scan", rclcpp::SensorDataQoS(), -// std::bind(&SlamGmapping::laserCallback, this, std::placeholders::_1)); + // 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)); @@ -198,7 +244,7 @@ bool SlamGmapping::initMapper(const sensor_msgs::msg::LaserScan::ConstSharedPtr if (fabs(fabs(up.point.z) - 1) > 0.001) { RCLCPP_INFO(this->get_logger(), - "Laser has to be mounted planar! Z-coordinate has to be 1 or -1, but gave: %.5f", up.point.z); + "Laser has to be mounted planar! Z-coordinate has to be 1 or -1, but gave: %.5f", up.point.z); return false; } @@ -243,9 +289,9 @@ bool SlamGmapping::initMapper(const sensor_msgs::msg::LaserScan::ConstSharedPtr } RCLCPP_DEBUG(this->get_logger(), "Laser angles in laser-frame: min: %.3f max: %.3f inc: %.3f", - scan->angle_min, scan->angle_max, scan->angle_increment); + scan->angle_min, scan->angle_max, scan->angle_increment); RCLCPP_DEBUG(this->get_logger(), "Laser angles in top-down centered laser-frame: min: %.3f max: %.3f inc: %.3f", - laser_angles_.front(), laser_angles_.back(), std::fabs(scan->angle_increment)); + laser_angles_.front(), laser_angles_.back(), std::fabs(scan->angle_increment)); GMapping::OrientedPoint gmap_pose(0, 0, 0); @@ -417,7 +463,7 @@ void SlamGmapping::updateMap(const sensor_msgs::msg::LaserScan::ConstSharedPtr s matcher.setgenerateMap(true); GMapping::GridSlamProcessor::Particle best = - gsp_->getParticles()[gsp_->getBestParticleIndex()]; + gsp_->getParticles()[gsp_->getBestParticleIndex()]; std_msgs::msg::Float64 entropy; entropy.data = computePoseEntropy(); if(entropy.data > 0.0) @@ -443,13 +489,13 @@ void SlamGmapping::updateMap(const sensor_msgs::msg::LaserScan::ConstSharedPtr s RCLCPP_DEBUG(this->get_logger(), "Trajectory tree:"); for(GMapping::GridSlamProcessor::TNode* n = best.node; - n; - n = n->parent) + n; + n = n->parent) { RCLCPP_DEBUG(this->get_logger(), " %.3f %.3f %.3f", - n->pose.x, - n->pose.y, - n->pose.theta); + n->pose.x, + n->pose.y, + n->pose.theta); if(!n->reading) { RCLCPP_DEBUG(this->get_logger(), "Reading is NULL"); @@ -471,7 +517,7 @@ void SlamGmapping::updateMap(const sensor_msgs::msg::LaserScan::ConstSharedPtr s xmax_ = wmax.x; ymax_ = wmax.y; RCLCPP_DEBUG(this->get_logger(), "map size is now %dx%d pixels (%f,%f)-(%f, %f)", smap.getMapSizeX(), smap.getMapSizeY(), - xmin_, ymin_, xmax_, ymax_); + xmin_, ymin_, xmax_, ymax_); map_.info.width = static_cast(smap.getMapSizeX()); map_.info.height = static_cast(smap.getMapSizeY()); @@ -516,7 +562,7 @@ void SlamGmapping::publishTransform() { map_to_odom_mutex_.lock(); rclcpp::Time tf_expiration = get_clock()->now() + rclcpp::Duration( - static_cast(static_cast(tf_delay_)), 0); + static_cast(static_cast(tf_delay_)), 0); geometry_msgs::msg::TransformStamped transform; transform.header.frame_id = map_frame_; transform.header.stamp = tf_expiration;