Skip to content

Commit

Permalink
linting and style changes
Browse files Browse the repository at this point in the history
  • Loading branch information
@acachathuranga
acachathuranga committed Nov 27, 2024
1 parent 6803c16 commit 90f5fb3
Show file tree
Hide file tree
Showing 4 changed files with 64 additions and 47 deletions.
4 changes: 3 additions & 1 deletion include/slam_toolbox/laser_utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -82,7 +82,9 @@ class LaserAssistant
const std::string & base_frame);
~LaserAssistant();
LaserMetadata toLaserMetadata(sensor_msgs::msg::LaserScan scan);
LaserMetadata toLaserMetadata(sensor_msgs::msg::LaserScan scan, geometry_msgs::msg::TransformStamped laser_pose);
LaserMetadata toLaserMetadata(
sensor_msgs::msg::LaserScan scan,
geometry_msgs::msg::TransformStamped laser_pose);

private:
karto::LaserRangeFinder * makeLaser(const double & mountingYaw);
Expand Down
22 changes: 14 additions & 8 deletions include/slam_toolbox/slam_toolbox_multirobot.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#define SLAM_TOOLBOX__SLAM_TOOLBOX_MULTIROBOT_HPP_

#include <memory>
#include <string>
#include "slam_toolbox/slam_toolbox_common.hpp"
#include "slam_toolbox/toolbox_msgs.hpp"

Expand All @@ -28,13 +29,16 @@ class MultiRobotSlamToolbox : public SlamToolbox
{
public:
explicit MultiRobotSlamToolbox(rclcpp::NodeOptions);
~MultiRobotSlamToolbox() {};
~MultiRobotSlamToolbox() {}

protected:
LocalizedRangeScan * addExternalScan(LaserRangeFinder * laser,
const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan, Pose2 & odom_pose);
void publishLocalizedScan(
const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan, const Pose2 &offset,
LocalizedRangeScan * addExternalScan(
LaserRangeFinder * laser,
const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan,
Pose2 & odom_pose);
void publishLocalizedScan(
const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan,
const Pose2 & offset,
const Pose2 & pose, const Matrix3 & cov,
const rclcpp::Time & t);

Expand All @@ -44,10 +48,12 @@ class MultiRobotSlamToolbox : public SlamToolbox
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<slam_toolbox::srv::DeserializePoseGraph::Request> req,
std::shared_ptr<slam_toolbox::srv::DeserializePoseGraph::Response> resp) override;
void localizedScanCallback(slam_toolbox::msg::LocalizedLaserScan::ConstSharedPtr localized_scan);
LaserRangeFinder * getLaser(const slam_toolbox::msg::LocalizedLaserScan::ConstSharedPtr localized_scan);
void localizedScanCallback(
slam_toolbox::msg::LocalizedLaserScan::ConstSharedPtr localized_scan);
LaserRangeFinder * getLaser(
const slam_toolbox::msg::LocalizedLaserScan::ConstSharedPtr localized_scan);
using SlamToolbox::getLaser;

std::shared_ptr<rclcpp::Publisher<slam_toolbox::msg::LocalizedLaserScan>> localized_scan_pub_;
rclcpp::Subscription<slam_toolbox::msg::LocalizedLaserScan>::SharedPtr localized_scan_sub_;
std::string localized_scan_topic_;
Expand Down
18 changes: 10 additions & 8 deletions src/laser_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -88,7 +88,9 @@ LaserMetadata LaserAssistant::toLaserMetadata(sensor_msgs::msg::LaserScan scan)
return toLaserMetadata(scan, readLaserPose());
}

LaserMetadata LaserAssistant::toLaserMetadata(sensor_msgs::msg::LaserScan scan, geometry_msgs::msg::TransformStamped laser_pose)
LaserMetadata LaserAssistant::toLaserMetadata(
sensor_msgs::msg::LaserScan scan,
geometry_msgs::msg::TransformStamped laser_pose)
{
scan_ = scan;
frame_ = scan_.header.frame_id;
Expand Down Expand Up @@ -191,13 +193,13 @@ bool LaserAssistant::isInverted(double & mountingYaw)
laser_pose_.transform.translation.y,
laser_pose_.transform.translation.z, mountingYaw);

tf2::Vector3 laser_orient;
tf2::Transform laser_pose;
tf2::convert(laser_pose_.transform, laser_pose);
laser_orient.setY(0.);
laser_orient.setZ(0.);
laser_orient.setZ(1 + laser_pose_.transform.translation.z); // TOOD can remove addition of laser_pose z component
laser_orient = laser_pose * laser_orient;
tf2::Vector3 laser_orient;
tf2::Transform laser_pose;
tf2::convert(laser_pose_.transform, laser_pose);
laser_orient.setY(0.);
laser_orient.setZ(0.);
laser_orient.setZ(1 + laser_pose_.transform.translation.z); // TOOD can remove addition of laser_pose z component
laser_orient = laser_pose * laser_orient;

if (laser_orient.vector.z <= 0) {
RCLCPP_DEBUG(
Expand Down
67 changes: 37 additions & 30 deletions src/slam_toolbox_multirobot.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,13 +24,14 @@ MultiRobotSlamToolbox::MultiRobotSlamToolbox(rclcpp::NodeOptions options)
: SlamToolbox(options), localized_scan_topic_("/localized_scan")
/*****************************************************************************/
{
current_ns_ = this->get_namespace() + 1;
current_ns_ = this->get_namespace() + 1;

localized_scan_pub_ = this->create_publisher<slam_toolbox::msg::LocalizedLaserScan>(
localized_scan_pub_ = this->create_publisher<slam_toolbox::msg::LocalizedLaserScan>(
localized_scan_topic_, 10);
localized_scan_sub_ = this->create_subscription<slam_toolbox::msg::LocalizedLaserScan>(
localized_scan_topic_, 10, std::bind(&MultiRobotSlamToolbox::localizedScanCallback,
this, std::placeholders::_1));
localized_scan_sub_ = this->create_subscription<slam_toolbox::msg::LocalizedLaserScan>(
localized_scan_topic_, 10, std::bind(
&MultiRobotSlamToolbox::localizedScanCallback,
this, std::placeholders::_1));
}

/*****************************************************************************/
Expand All @@ -51,30 +52,33 @@ void MultiRobotSlamToolbox::laserCallback(
LaserRangeFinder * laser = getLaser(scan);

if (!laser) {
RCLCPP_WARN(get_logger(), "Failed to create laser device for"
RCLCPP_WARN(
get_logger(), "Failed to create laser device for"
" %s; discarding scan", scan->header.frame_id.c_str());
return;
}

LocalizedRangeScan * range_scan = addScan(laser, scan, pose);
if (range_scan != nullptr)
{
if (range_scan != nullptr) {
Matrix3 covariance;
covariance.SetToIdentity();
publishLocalizedScan(scan, laser->GetOffsetPose(),
range_scan->GetOdometricPose(), covariance, scan->header.stamp);
publishLocalizedScan(
scan, laser->GetOffsetPose(),
range_scan->GetOdometricPose(), covariance, scan->header.stamp);
}
}

/*****************************************************************************/
void MultiRobotSlamToolbox::localizedScanCallback(
slam_toolbox::msg::LocalizedLaserScan::ConstSharedPtr localized_scan)
{
std::string scan_ns = localized_scan->scan.header.frame_id.substr(0,
localized_scan->scan.header.frame_id.find('/'));
if (scan_ns == current_ns_) return; // Ignore callbacks from ourself
std::string scan_ns = localized_scan->scan.header.frame_id.substr(
0, localized_scan->scan.header.frame_id.find('/'));
if (scan_ns == current_ns_) {
return; // Ignore callbacks from ourself
}

sensor_msgs::msg::LaserScan::ConstSharedPtr scan =
sensor_msgs::msg::LaserScan::ConstSharedPtr scan =
std::make_shared<sensor_msgs::msg::LaserScan>(localized_scan->scan);
Pose2 pose;
pose.SetX(localized_scan->pose.pose.pose.position.x);
Expand All @@ -85,14 +89,14 @@ void MultiRobotSlamToolbox::localizedScanCallback(

LaserRangeFinder * laser = getLaser(localized_scan);
if (!laser) {
RCLCPP_WARN(get_logger(), "Failed to create device for received localizedScanner"
RCLCPP_WARN(
get_logger(), "Failed to create device for received localizedScanner"
" %s; discarding scan", scan->header.frame_id.c_str());
return;
}
LocalizedRangeScan * range_scan = addExternalScan(laser, scan, pose);

if (range_scan != nullptr)
{

if (range_scan != nullptr) {
// Publish transform
pose = range_scan->GetCorrectedPose();
tf2::Quaternion q(0., 0., 0., 1.0);
Expand Down Expand Up @@ -134,7 +138,8 @@ LocalizedRangeScan * MultiRobotSlamToolbox::addExternalScan(
} else if (processor_type_ == PROCESS_NEAR_REGION) {
boost::mutex::scoped_lock l(pose_mutex_);
if (!process_near_pose_) {
RCLCPP_ERROR(get_logger(), "Process near region called without a "
RCLCPP_ERROR(
get_logger(), "Process near region called without a "
"valid region request. Ignoring scan.");
return nullptr;
}
Expand All @@ -146,8 +151,8 @@ LocalizedRangeScan * MultiRobotSlamToolbox::addExternalScan(
update_reprocessing_transform = true;
processor_type_ = PROCESS;
} else {
RCLCPP_FATAL(get_logger(),
"SlamToolbox: No valid processor type set! Exiting.");
RCLCPP_FATAL(
get_logger(), "SlamToolbox: No valid processor type set! Exiting.");
exit(-1);
}

Expand All @@ -173,11 +178,12 @@ LaserRangeFinder * MultiRobotSlamToolbox::getLaser(
const std::string & frame = localized_scan->scan.header.frame_id;
if (lasers_.find(frame) == lasers_.end()) {
try {
lasers_[frame] = laser_assistant_->toLaserMetadata(localized_scan->scan,
localized_scan->scanner_offset);
lasers_[frame] = laser_assistant_->toLaserMetadata(
localized_scan->scan, localized_scan->scanner_offset);
dataset_->Add(lasers_[frame].getLaser(), true);
} catch (tf2::TransformException & e) {
RCLCPP_ERROR(get_logger(), "Failed to compute laser pose[%s], "
RCLCPP_ERROR(
get_logger(), "Failed to compute laser pose[%s], "
"aborting initialization (%s)", frame.c_str(), e.what());
return nullptr;
}
Expand All @@ -187,15 +193,15 @@ LaserRangeFinder * MultiRobotSlamToolbox::getLaser(
}

/*****************************************************************************/
void MultiRobotSlamToolbox::publishLocalizedScan(
void MultiRobotSlamToolbox::publishLocalizedScan(
const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan,
const Pose2 & offset,
const Pose2 & pose,
const Matrix3 & cov,
const rclcpp::Time & t)
/*****************************************************************************/
{
slam_toolbox::msg::LocalizedLaserScan scan_msg;
slam_toolbox::msg::LocalizedLaserScan scan_msg;

scan_msg.scan = *scan;

Expand All @@ -218,11 +224,11 @@ void MultiRobotSlamToolbox::publishLocalizedScan(
scan_msg.pose.header.stamp = t;

// Set prefixed frame names
scan_msg.scan.header.frame_id = (*(scan->header.frame_id.cbegin()) == '/') ?
scan_msg.scan.header.frame_id = (*(scan->header.frame_id.cbegin()) == '/') ?
current_ns_ + scan->header.frame_id :
current_ns_ + "/" + scan->header.frame_id;

scan_msg.pose.header.frame_id = (*(map_frame_.cbegin()) == '/') ?
scan_msg.pose.header.frame_id = (*(map_frame_.cbegin()) == '/') ?
current_ns_ + map_frame_ :
current_ns_ + "/" + map_frame_;

Expand All @@ -243,12 +249,13 @@ bool MultiRobotSlamToolbox::deserializePoseGraphCallback(
/*****************************************************************************/
{
if (req->match_type == procType::LOCALIZE_AT_POSE) {
RCLCPP_WARN(get_logger(), "Requested a localization deserialization "
RCLCPP_WARN(
get_logger(), "Requested a localization deserialization "
"in non-localization mode.");
return false;
}

return SlamToolbox::deserializePoseGraphCallback(request_header, req, resp);
}

}
} // namespace slam_toolbox

0 comments on commit 90f5fb3

Please sign in to comment.