Skip to content

Commit bc29e07

Browse files
nikoskoukis-slamcorenikoskoukis-slamcore
committed
Refactor code, publish algorithm parameters as ROS parameters
1 parent 3682051 commit bc29e07

2 files changed

Lines changed: 125 additions & 52 deletions

File tree

‎slam_gmapping/include/slam_gmapping/slam_gmapping.h‎

Lines changed: 12 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323
#ifndef SLAM_GMAPPING_SLAM_GMAPPING_H_
2424
#define SLAM_GMAPPING_SLAM_GMAPPING_H_
2525

26+
#include <system_error>
2627
#include <mutex>
2728
#include <thread>
2829
#include <memory>
@@ -53,13 +54,17 @@ class SlamGmapping : public rclcpp::Node{
5354
SlamGmapping();
5455
~SlamGmapping() override;
5556

56-
void init();
57-
void startLiveSlam();
57+
std::error_code init();
58+
std::error_code startLiveSlam();
59+
void doPublish();
60+
void publishLatestPose();
5861
void publishTransform();
5962
void laserCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr scan);
6063
void publishLoop(double transform_publish_period);
6164

6265
private:
66+
std::error_code initParameters();
67+
6368
rclcpp::Node::SharedPtr node_;
6469
rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr entropy_publisher_;
6570
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr sst_;
@@ -102,16 +107,18 @@ class SlamGmapping : public rclcpp::Node{
102107
std::shared_ptr<std::thread> transform_thread_;
103108

104109
std::string base_frame_;
105-
std::string laser_frame_;
106110
std::string map_frame_;
107111
std::string odom_frame_;
112+
std::string laser_frame_;
108113

109114
void updateMap(sensor_msgs::msg::LaserScan::ConstSharedPtr scan);
110115
bool getOdomPose(GMapping::OrientedPoint& gmap_pose, const rclcpp::Time& t);
111116
bool initMapper(sensor_msgs::msg::LaserScan::ConstSharedPtr scan);
112117
bool addScan(sensor_msgs::msg::LaserScan::ConstSharedPtr scan, GMapping::OrientedPoint& gmap_pose);
113118
double computePoseEntropy();
114119

120+
std::unique_ptr<rclcpp::SyncParametersClient> parameters_client_;
121+
115122
// Parameters used by GMapping
116123
double maxRange_;
117124
double maxUrange_;
@@ -147,8 +154,8 @@ class SlamGmapping : public rclcpp::Node{
147154

148155
unsigned long int seed_;
149156

150-
double transform_publish_period_;
151-
double tf_delay_;
157+
double transform_publish_period_ = 0.05;
158+
double tf_delay_ = transform_publish_period_;
152159

153160
// latest pose in the map frame
154161
geometry_msgs::msg::PoseWithCovarianceStamped latestPose_;

‎slam_gmapping/src/slam_gmapping.cpp‎

Lines changed: 113 additions & 47 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@
2222
//
2323

2424
#include "slam_gmapping/slam_gmapping.h"
25+
using namespace std::chrono_literals;
2526

2627
#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
2728

@@ -40,77 +41,102 @@ SlamGmapping::SlamGmapping():
4041
tfB_ = std::make_shared<tf2_ros::TransformBroadcaster>(node_);
4142
map_to_odom_.setIdentity();
4243
seed_ = static_cast<unsigned long>(time(nullptr));
43-
init();
44-
startLiveSlam();
4544
}
4645

47-
void SlamGmapping::init() {
46+
std::error_code SlamGmapping::init() {
4847
gsp_ = new GMapping::GridSlamProcessor();
4948

49+
// variables initialisation
5050
gsp_laser_ = nullptr;
5151
gsp_odom_ = nullptr;
5252
got_first_scan_ = false;
5353
got_map_ = false;
5454

5555
throttle_scans_ = 1;
56-
base_frame_ = "base_link";
57-
map_frame_ = "map";
58-
odom_frame_ = "odom";
59-
transform_publish_period_ = 0.05;
60-
6156
map_update_interval_ = tf2::durationFromSec(0.5);
62-
maxUrange_ = 80.0; maxRange_ = 0.0;
63-
minimum_score_ = 0;
64-
sigma_ = 0.05;
65-
kernelSize_ = 1;
66-
lstep_ = 0.05;
67-
astep_ = 0.05;
68-
iterations_ = 5;
69-
lsigma_ = 0.075;
70-
ogain_ = 3.0;
71-
lskip_ = 0;
72-
srr_ = 0.1;
73-
srt_ = 0.2;
74-
str_ = 0.1;
75-
stt_ = 0.2;
76-
linearUpdate_ = 1.0;
77-
angularUpdate_ = 0.5;
78-
temporalUpdate_ = 1.0;
79-
resampleThreshold_ = 0.5;
80-
particles_ = 30;
81-
xmin_ = -10.0;
82-
ymin_ = -10.0;
83-
xmax_ = 10.0;
84-
ymax_ = 10.0;
85-
delta_ = 0.05;
86-
occ_thresh_ = 0.25;
87-
llsamplerange_ = 0.01;
88-
llsamplestep_ = 0.01;
89-
lasamplerange_ = 0.005;
90-
lasamplestep_ = 0.005;
91-
tf_delay_ = transform_publish_period_;
57+
58+
// ROS parameters initialisation
59+
const auto ec = initParameters();
60+
if (ec) { return ec; }
61+
62+
return std::error_code();
63+
} // end of init
64+
65+
66+
std::error_code SlamGmapping::initParameters()
67+
{
68+
parameters_client_ = std::make_unique<rclcpp::SyncParametersClient>(node_);
69+
70+
// Query and initialise gmapping parameters
71+
while (!parameters_client_->wait_for_service(1s)) {
72+
if (!rclcpp::ok()) {
73+
RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting.");
74+
return std::make_error_code(std::errc::io_error);
75+
}
76+
RCLCPP_INFO(node_->get_logger(), "service not available, waiting again...");
77+
}
78+
79+
// frames
80+
base_frame_ = parameters_client_->get_parameter<std::string>("base_frame", "base_link");
81+
map_frame_ = parameters_client_->get_parameter<std::string>("map_frame", "map");
82+
odom_frame_ = parameters_client_->get_parameter<std::string>("odom_frame", "odom");
83+
84+
// various parameters
85+
maxUrange_ = parameters_client_->get_parameter("maxUrange", 80.0);
86+
maxRange_ = parameters_client_->get_parameter("maxRange", 0.0);
87+
minimum_score_ = parameters_client_->get_parameter("minimum_score", 0);
88+
sigma_ = parameters_client_->get_parameter("sigma", 0.05);
89+
kernelSize_ = parameters_client_->get_parameter("kernelSize", 1);
90+
lstep_ = parameters_client_->get_parameter("lstep", 0.05);
91+
astep_ = parameters_client_->get_parameter("astep", 0.05);
92+
iterations_ = parameters_client_->get_parameter("iterations", 5);
93+
lsigma_ = parameters_client_->get_parameter("lsigma", 0.075);
94+
ogain_ = parameters_client_->get_parameter("ogain", 3.0);
95+
lskip_ = parameters_client_->get_parameter("lskip", 0);
96+
srr_ = parameters_client_->get_parameter("srr", 0.1);
97+
srt_ = parameters_client_->get_parameter("srt", 0.2);
98+
str_ = parameters_client_->get_parameter("str", 0.1);
99+
stt_ = parameters_client_->get_parameter("stt", 0.2);
100+
linearUpdate_ = parameters_client_->get_parameter("linearUpdate", 1.0);
101+
angularUpdate_ = parameters_client_->get_parameter("angularUpdate", 0.5);
102+
temporalUpdate_ = parameters_client_->get_parameter("temporalUpdate", 1.0);
103+
resampleThreshold_ = parameters_client_->get_parameter("resampleThreshold", 0.5);
104+
particles_ = parameters_client_->get_parameter("particles", 30);
105+
xmin_ = parameters_client_->get_parameter("xmin", -10.0);
106+
ymin_ = parameters_client_->get_parameter("ymin", -10.0);
107+
xmax_ = parameters_client_->get_parameter("xmax", 10.0);
108+
ymax_ = parameters_client_->get_parameter("ymax", 10.0);
109+
delta_ = parameters_client_->get_parameter("delta", 0.05);
110+
occ_thresh_ = parameters_client_->get_parameter("occ", 0.25);
111+
llsamplerange_ = parameters_client_->get_parameter("llsamplerange", 0.01);
112+
llsamplestep_ = parameters_client_->get_parameter("llsamplestep", 0.01);
113+
lasamplerange_ = parameters_client_->get_parameter("lasamplerange", 0.005);
114+
lasamplestep_ = parameters_client_->get_parameter("lasamplestep", 0.005);
115+
116+
return std::error_code();
92117
}
93118

94-
void SlamGmapping::startLiveSlam() {
119+
std::error_code SlamGmapping::startLiveSlam() {
95120
entropy_publisher_ = this->create_publisher<std_msgs::msg::Float64>("entropy");
96121
sst_ = this->create_publisher<nav_msgs::msg::OccupancyGrid>("map");
97122
sstm_ = this->create_publisher<nav_msgs::msg::MapMetaData>("map_metadata");
98123
pose_publisher_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("pose");
99-
scan_filter_sub_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::LaserScan>>
100-
(node_, "scan");
124+
scan_filter_sub_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::LaserScan>>(node_, "scan");
101125
scan_filter_ = std::make_shared<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>>
102126
(*scan_filter_sub_, *buffer_, odom_frame_, 10, node_);
103127
scan_filter_->registerCallback(std::bind(&SlamGmapping::laserCallback, this, std::placeholders::_1));
104128
transform_thread_ = std::make_shared<std::thread>
105129
(std::bind(&SlamGmapping::publishLoop, this, transform_publish_period_));
130+
131+
return std::error_code();
106132
}
107133

108134
void SlamGmapping::publishLoop(double transform_publish_period){
109135
if (transform_publish_period == 0)
110136
return;
111137
rclcpp::Rate r(1.0 / transform_publish_period);
112138
while (rclcpp::ok()) {
113-
publishTransform();
139+
doPublish();
114140
r.sleep();
115141
}
116142
}
@@ -524,12 +550,24 @@ void SlamGmapping::updateMap(const sensor_msgs::msg::LaserScan::ConstSharedPtr s
524550
map_mutex_.unlock();
525551
}
526552

527-
void SlamGmapping::publishTransform()
553+
void SlamGmapping::doPublish()
554+
{
555+
publishTransform();
556+
publishLatestPose();
557+
}
558+
559+
void SlamGmapping::publishLatestPose()
528560
{
529-
map_to_odom_mutex_.lock();
561+
std::lock_guard<std::mutex> g(map_to_odom_mutex_);
530562

531563
// publish the latest pose
532564
pose_publisher_->publish(latestPose_);
565+
}
566+
567+
void SlamGmapping::publishTransform()
568+
{
569+
std::lock_guard<std::mutex> g(map_to_odom_mutex_);
570+
533571

534572
rclcpp::Time tf_expiration = get_clock()->now() + rclcpp::Duration(
535573
static_cast<int32_t>(static_cast<rcl_duration_value_t>(tf_delay_)), 0);
@@ -544,14 +582,42 @@ void SlamGmapping::publishTransform()
544582
catch (tf2::LookupException& te){
545583
RCLCPP_INFO(this->get_logger(), te.what());
546584
}
547-
map_to_odom_mutex_.unlock();
548585
}
549586

550-
int main(int argc, char* argv[])
587+
static rclcpp::Logger logger = rclcpp::get_logger("main");
588+
589+
int main(int argc, char* argv[]) try
551590
{
552591
rclcpp::init(argc, argv);
553592

554593
auto slam_gmapping_node = std::make_shared<SlamGmapping>();
594+
{
595+
const auto ec = slam_gmapping_node->init();
596+
if (ec)
597+
{
598+
RCLCPP_ERROR(logger, "%s / %s / %s", ec.category().name(), ec.value(), ec.message());
599+
return -1;
600+
}
601+
}
602+
{
603+
const auto ec = slam_gmapping_node->startLiveSlam();
604+
if (ec)
605+
{
606+
RCLCPP_ERROR(logger, "%s / %s / %s", ec.category().name(), ec.value(), ec.message());
607+
return -1;
608+
}
609+
}
610+
555611
rclcpp::spin(slam_gmapping_node);
556-
return(0);
612+
return 0;
613+
}
614+
catch(const std::exception& e)
615+
{
616+
RCLCPP_FATAL(logger, "%s%s\n\n", "Finished with a known exception:", e.what());
617+
return -1;
618+
}
619+
catch(...)
620+
{
621+
RCLCPP_FATAL(logger, "%s", "Uncaught unknown exception!\n");
622+
return -1;
557623
}

0 commit comments

Comments
 (0)