diff --git a/CMakeLists.txt b/CMakeLists.txt index 539655ee3..b1be251d5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -74,6 +74,7 @@ set(libraries toolbox_common SlamToolboxPlugin ceres_solver_plugin + multirobot_slam_toolbox async_slam_toolbox sync_slam_toolbox localization_slam_toolbox @@ -103,6 +104,7 @@ include_directories(include lib/karto_sdk/include add_definitions(${EIGEN3_DEFINITIONS}) rosidl_generate_interfaces(${PROJECT_NAME} + msg/LocalizedLaserScan.msg srvs/Pause.srv srvs/Reset.srv srvs/ClearQueue.srv @@ -114,7 +116,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} srvs/AddSubmap.srv srvs/DeserializePoseGraph.srv srvs/SerializePoseGraph.srv - DEPENDENCIES builtin_interfaces geometry_msgs std_msgs nav_msgs visualization_msgs + DEPENDENCIES builtin_interfaces geometry_msgs std_msgs nav_msgs visualization_msgs sensor_msgs ) #### rviz Plugin @@ -157,6 +159,11 @@ rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typ target_link_libraries(toolbox_common "${cpp_typesupport_target}") #### Mapping executibles +add_library(multirobot_slam_toolbox src/slam_toolbox_multirobot.cpp) +target_link_libraries(multirobot_slam_toolbox toolbox_common kartoSlamToolbox ${Boost_LIBRARIES}) +add_executable(multirobot_slam_toolbox_node src/slam_toolbox_multirobot_node.cpp) +target_link_libraries(multirobot_slam_toolbox_node multirobot_slam_toolbox) + add_library(async_slam_toolbox src/slam_toolbox_async.cpp) target_link_libraries(async_slam_toolbox toolbox_common kartoSlamToolbox ${Boost_LIBRARIES}) add_executable(async_slam_toolbox_node src/slam_toolbox_async_node.cpp) @@ -203,7 +210,8 @@ rclcpp_components_register_nodes(lifelong_slam_toolbox "slam_toolbox::LifelongSl rclcpp_components_register_nodes(map_and_localization_slam_toolbox "slam_toolbox::MapAndLocalizationSlamToolbox") #### Install -install(TARGETS async_slam_toolbox_node +install(TARGETS multirobot_slam_toolbox_node + async_slam_toolbox_node sync_slam_toolbox_node localization_slam_toolbox_node map_and_localization_slam_toolbox_node diff --git a/README.md b/README.md index ab8209878..911be2404 100644 --- a/README.md +++ b/README.md @@ -111,6 +111,37 @@ To minimize the amount of changes required for moving to this mode over AMCL, we In summary, this approach I dub `elastic pose-graph localization` is where we take existing map pose-graphs and localized with-in them with a rolling window of recent scans. This way we can localize in an existing map using the scan matcher, but not update the underlaying map long-term should something go wrong. It can be considered a replacement to AMCL and results is not needing any .pgm maps ever again. The lifelong mapping/continuous slam mode above will do better if you'd like to modify the underlying graph while moving. This method of localization might not be suitable for all applications, it does require quite a bit of tuning for your particular robot and needs high quality odometry. If in doubt, you're always welcome to use other 2D map localizers in the ecosystem like AMCL. For most beginners or users looking for a good out of the box experience, I'd recommend AMCL. +# Multi-Robot SLAM +![multirobot_slam](/images/multi-robot_mapping.gif?raw=true "Multi-Robot SLAM") + +*multirobot_slam_toolbox_node* extends slam_toolbox for multi-robot mapping and localization. Slam_toolbox instances are run on each robot under unique namespaces and pose graph data are shared between the nodes through */localized_scan* topic. Each slam_toolbox instance publishes transform of host robot and peers in the network. Peer transform frame names are prefixed with the namespace of the peer. + +Initially the robots should start close to each other, in the same orientation, as scan matching is used to find the connection between different pose graphs. This however is not required in simulation environments such as gazebo, as all robots' odometry is referenced to a gazebo's global frame. + +![multirobot_slam_architecture](/images/multi-robot_architecture.png?raw=true "Multi-Robot SLAM Node Architecture") + +### Running Multi-Robot SLAM with turtlebot3 simulation + +Install and launch turtlebot3 multirobot simulation as described in the [navigation2 documentation](https://navigation.ros.org/getting_started/index.html) + +```shell +ros2 launch nav2_bringup multi_tb3_simulation_launch.py headless:=False +``` + +In two seperate terminals, launch two *multirobot_slam_toolbox_node* instances for robot1 and robot2. + +```shell +ros2 launch slam_toolbox online_async_multirobot_launch.py namespace:=robot1 +ros2 launch slam_toolbox online_async_multirobot_launch.py namespace:=robot2 +``` + +In another two seperate terminals, run *teleop_twist_keyboard* to control the robots. The merged map will appear on both rviz windows when moving the robots around in the environment. + +```shell +ros2 run teleop_twist_keyboard teleop_twist_keyboard __ns:=/robot1 +ros2 run teleop_twist_keyboard teleop_twist_keyboard __ns:=/robot2 +``` + ## Tools ### Plugin based Optimizers diff --git a/config/mapper_params_online_multi_async.yaml b/config/mapper_params_online_multi_async.yaml new file mode 100644 index 000000000..7acb3671e --- /dev/null +++ b/config/mapper_params_online_multi_async.yaml @@ -0,0 +1,73 @@ +$(var namespace)/slam_toolbox: + ros__parameters: + + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_footprint + scan_topic: scan + mode: mapping #localization + + # if you'd like to immediately start continuing a map at a given pose + # or at the dock, but they are mutually exclusive, if pose is given + # will use pose + #map_file_name: test_steve + # map_start_pose: [0.0, 0.0, 0.0] + #map_start_at_dock: true + + debug_logging: false + throttle_scans: 1 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 5.0 + resolution: 0.05 + max_laser_range: 20.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.2 + tf_buffer_duration: 30.0 + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + enable_interactive_mode: true + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.5 + minimum_travel_heading: 0.5 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true \ No newline at end of file diff --git a/images/multi-robot_architecture.png b/images/multi-robot_architecture.png new file mode 100644 index 000000000..32e7d3592 Binary files /dev/null and b/images/multi-robot_architecture.png differ diff --git a/images/multi-robot_mapping.gif b/images/multi-robot_mapping.gif new file mode 100644 index 000000000..fc088be24 Binary files /dev/null and b/images/multi-robot_mapping.gif differ diff --git a/include/slam_toolbox/laser_utils.hpp b/include/slam_toolbox/laser_utils.hpp index f7f81e6c6..02099ad7c 100644 --- a/include/slam_toolbox/laser_utils.hpp +++ b/include/slam_toolbox/laser_utils.hpp @@ -82,10 +82,14 @@ 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); private: karto::LaserRangeFinder * makeLaser(const double & mountingYaw); bool isInverted(double & mountingYaw); + geometry_msgs::msg::TransformStamped readLaserPose(); rclcpp::Logger logger_; rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface_; diff --git a/include/slam_toolbox/slam_toolbox_multirobot.hpp b/include/slam_toolbox/slam_toolbox_multirobot.hpp new file mode 100644 index 000000000..d1eeac3ce --- /dev/null +++ b/include/slam_toolbox/slam_toolbox_multirobot.hpp @@ -0,0 +1,65 @@ +/* + * multirobot_slam_toolbox + * Copyright Work Modifications (c) 2023, Achala Athukorala + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +#ifndef SLAM_TOOLBOX__SLAM_TOOLBOX_MULTIROBOT_HPP_ +#define SLAM_TOOLBOX__SLAM_TOOLBOX_MULTIROBOT_HPP_ + +#include +#include +#include "slam_toolbox/slam_toolbox_common.hpp" +#include "slam_toolbox/toolbox_msgs.hpp" + +namespace slam_toolbox +{ + +class MultiRobotSlamToolbox : public SlamToolbox +{ +public: + explicit MultiRobotSlamToolbox(rclcpp::NodeOptions); + ~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, + const Pose2 & pose, const Matrix3 & cov, + const rclcpp::Time & t); + + // callbacks + void laserCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr scan) override; + bool deserializePoseGraphCallback( + const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr resp) override; + 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> localized_scan_pub_; + rclcpp::Subscription::SharedPtr localized_scan_sub_; + std::string localized_scan_topic_; + std::string current_ns_; +}; + +} // namespace slam_toolbox + +#endif // SLAM_TOOLBOX__SLAM_TOOLBOX_MULTIROBOT_HPP_ diff --git a/include/slam_toolbox/toolbox_msgs.hpp b/include/slam_toolbox/toolbox_msgs.hpp index c8886da23..e3bb19d07 100644 --- a/include/slam_toolbox/toolbox_msgs.hpp +++ b/include/slam_toolbox/toolbox_msgs.hpp @@ -40,5 +40,6 @@ #include "slam_toolbox/srv/deserialize_pose_graph.hpp" #include "slam_toolbox/srv/merge_maps.hpp" #include "slam_toolbox/srv/add_submap.hpp" +#include "slam_toolbox/msg/localized_laser_scan.hpp" #endif // SLAM_TOOLBOX__TOOLBOX_MSGS_HPP_ diff --git a/launch/online_async_multirobot_launch.py b/launch/online_async_multirobot_launch.py new file mode 100644 index 000000000..4da170800 --- /dev/null +++ b/launch/online_async_multirobot_launch.py @@ -0,0 +1,51 @@ +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.descriptions import ParameterFile +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + pkg_dir = get_package_share_directory("slam_toolbox") + + use_sim_time = LaunchConfiguration('use_sim_time') + namespace = LaunchConfiguration('namespace') + + # Topic remappings + remappings = [ ('/map', 'map'), + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ('/map_metadata', 'map_metadata')] + + declare_use_sim_time_argument = DeclareLaunchArgument( + 'use_sim_time', + default_value='true', + description='Use simulation/Gazebo clock') + + declare_robot_name_argument = DeclareLaunchArgument( + 'namespace', + default_value='robot1', + description='Robot Name / Namespace') + + start_async_slam_toolbox_node = Node( + package='slam_toolbox', + executable='multirobot_slam_toolbox_node', + name='slam_toolbox', + namespace=namespace, + output='screen', + remappings=remappings, + parameters=[ + ParameterFile(os.path.join(pkg_dir, 'config', 'mapper_params_online_multi_async.yaml'), allow_substs=True), + {'use_sim_time': use_sim_time}] + ) + + ld = LaunchDescription() + ld.add_action(declare_robot_name_argument) + + ld.add_action(declare_use_sim_time_argument) + ld.add_action(start_async_slam_toolbox_node) + + return ld diff --git a/msg/LocalizedLaserScan.msg b/msg/LocalizedLaserScan.msg new file mode 100644 index 000000000..991c7b4e3 --- /dev/null +++ b/msg/LocalizedLaserScan.msg @@ -0,0 +1,7 @@ +# Single scan from a planar laser range-finder and the pose of the robot + +sensor_msgs/LaserScan scan +# Scanner pose in robot base_frame +geometry_msgs/TransformStamped scanner_offset +# Robot base_frame pose in a global (map) frame +geometry_msgs/PoseWithCovarianceStamped pose diff --git a/package.xml b/package.xml index 092630ed8..2cf36b19e 100644 --- a/package.xml +++ b/package.xml @@ -69,10 +69,7 @@ nav2_map_server builtin_interfaces rosidl_default_generators - lifecycle_msgs - bondcpp - bond - rclcpp_lifecycle + rosidl_default_runtime libqt5-core libqt5-gui diff --git a/src/laser_utils.cpp b/src/laser_utils.cpp index b2d9f2d0f..c8d56bdf6 100644 --- a/src/laser_utils.cpp +++ b/src/laser_utils.cpp @@ -85,6 +85,16 @@ LaserMetadata LaserAssistant::toLaserMetadata(sensor_msgs::msg::LaserScan scan) { scan_ = scan; frame_ = scan_.header.frame_id; + return toLaserMetadata(scan, readLaserPose()); +} + +LaserMetadata LaserAssistant::toLaserMetadata( + sensor_msgs::msg::LaserScan scan, + geometry_msgs::msg::TransformStamped laser_pose) +{ + scan_ = scan; + frame_ = scan_.header.frame_id; + laser_pose_ = laser_pose; double mountingYaw; bool inverted = isInverted(mountingYaw); @@ -95,9 +105,10 @@ LaserMetadata LaserAssistant::toLaserMetadata(sensor_msgs::msg::LaserScan scan) karto::LaserRangeFinder * LaserAssistant::makeLaser(const double & mountingYaw) { + std::string laser_namespace = scan_.header.frame_id.substr(0, scan_.header.frame_id.find('/')); karto::LaserRangeFinder * laser = karto::LaserRangeFinder::CreateLaserRangeFinder( - karto::LaserRangeFinder_Custom, karto::Name("Custom Described Lidar")); + karto::LaserRangeFinder_Custom, karto::Name("Custom Described Lidar : " + laser_namespace)); laser->SetOffsetPose(karto::Pose2(laser_pose_.transform.translation.x, laser_pose_.transform.translation.y, mountingYaw)); @@ -174,12 +185,6 @@ karto::LaserRangeFinder * LaserAssistant::makeLaser(const double & mountingYaw) bool LaserAssistant::isInverted(double & mountingYaw) { - geometry_msgs::msg::TransformStamped laser_ident; - laser_ident.header.stamp = scan_.header.stamp; - laser_ident.header.frame_id = frame_; - laser_ident.transform.rotation.w = 1.0; - - laser_pose_ = tf_->transform(laser_ident, base_frame_); mountingYaw = tf2::getYaw(laser_pose_.transform.rotation); RCLCPP_DEBUG( @@ -188,12 +193,13 @@ bool LaserAssistant::isInverted(double & mountingYaw) laser_pose_.transform.translation.y, laser_pose_.transform.translation.z, mountingYaw); - geometry_msgs::msg::Vector3Stamped laser_orient; - laser_orient.vector.z = laser_orient.vector.y = 0.; - laser_orient.vector.z = 1 + laser_pose_.transform.translation.z; - laser_orient.header.stamp = scan_.header.stamp; - laser_orient.header.frame_id = base_frame_; - laser_orient = tf_->transform(laser_orient, frame_); + 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( @@ -204,6 +210,16 @@ bool LaserAssistant::isInverted(double & mountingYaw) return false; } +geometry_msgs::msg::TransformStamped LaserAssistant::readLaserPose() +{ + geometry_msgs::msg::TransformStamped laser_ident; + laser_ident.header.stamp = scan_.header.stamp; + laser_ident.header.frame_id = frame_; + laser_ident.transform.rotation.w = 1.0; + + return tf_->transform(laser_ident, base_frame_); +} + ScanHolder::ScanHolder(std::map & lasers) : lasers_(lasers) { diff --git a/src/slam_toolbox_multirobot.cpp b/src/slam_toolbox_multirobot.cpp new file mode 100644 index 000000000..ab99ed9ce --- /dev/null +++ b/src/slam_toolbox_multirobot.cpp @@ -0,0 +1,261 @@ +/* + * multirobot_slam_toolbox + * Copyright Work Modifications (c) 2023, Achala Athukorala + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +#include "slam_toolbox/slam_toolbox_multirobot.hpp" + +namespace slam_toolbox +{ + +/*****************************************************************************/ +MultiRobotSlamToolbox::MultiRobotSlamToolbox(rclcpp::NodeOptions options) +: SlamToolbox(options), localized_scan_topic_("/localized_scan") +/*****************************************************************************/ +{ + current_ns_ = this->get_namespace() + 1; + + localized_scan_pub_ = this->create_publisher( + localized_scan_topic_, 10); + localized_scan_sub_ = this->create_subscription( + localized_scan_topic_, 10, std::bind( + &MultiRobotSlamToolbox::localizedScanCallback, + this, std::placeholders::_1)); +} + +/*****************************************************************************/ +void MultiRobotSlamToolbox::laserCallback( + sensor_msgs::msg::LaserScan::ConstSharedPtr scan) +/*****************************************************************************/ +{ + // store scan header + scan_header = scan->header; + // no odom info + Pose2 pose; + if (!pose_helper_->getOdomPose(pose, scan->header.stamp)) { + RCLCPP_WARN(get_logger(), "Failed to compute odom pose"); + return; + } + + // ensure the laser can be used + LaserRangeFinder * laser = getLaser(scan); + + if (!laser) { + 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) { + Matrix3 covariance; + covariance.SetToIdentity(); + 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 + } + + sensor_msgs::msg::LaserScan::ConstSharedPtr scan = + std::make_shared(localized_scan->scan); + Pose2 pose; + pose.SetX(localized_scan->pose.pose.pose.position.x); + pose.SetY(localized_scan->pose.pose.pose.position.y); + tf2::Quaternion quat_tf; + tf2::convert(localized_scan->pose.pose.pose.orientation, quat_tf); + pose.SetHeading(tf2::getYaw(quat_tf)); + + LaserRangeFinder * laser = getLaser(localized_scan); + if (!laser) { + 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) { + // Publish transform + pose = range_scan->GetCorrectedPose(); + tf2::Quaternion q(0., 0., 0., 1.0); + geometry_msgs::msg::TransformStamped tf_msg; + q.setRPY(0., 0., pose.GetHeading()); + tf2::Transform transform(q, tf2::Vector3(pose.GetX(), pose.GetY(), 0.0)); + tf2::toMsg(transform, tf_msg.transform); + tf_msg.header.frame_id = map_frame_; + tf_msg.header.stamp = localized_scan->pose.header.stamp; + tf_msg.child_frame_id = localized_scan->scanner_offset.header.frame_id; + tfB_->sendTransform(tf_msg); + } +} + +/*****************************************************************************/ +LocalizedRangeScan * MultiRobotSlamToolbox::addExternalScan( + LaserRangeFinder * laser, + const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan, + Pose2 & odom_pose) +/*****************************************************************************/ +{ + // get our localized range scan + LocalizedRangeScan * range_scan = getLocalizedRangeScan( + laser, scan, odom_pose); + + // Add the localized range scan to the smapper + boost::mutex::scoped_lock lock(smapper_mutex_); + bool processed = false, update_reprocessing_transform = false; + + Matrix3 covariance; + covariance.SetToIdentity(); + + if (processor_type_ == PROCESS) { + processed = smapper_->getMapper()->Process(range_scan, &covariance); + } else if (processor_type_ == PROCESS_FIRST_NODE) { + processed = smapper_->getMapper()->ProcessAtDock(range_scan, &covariance); + processor_type_ = PROCESS; + update_reprocessing_transform = true; + } 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 " + "valid region request. Ignoring scan."); + return nullptr; + } + range_scan->SetOdometricPose(*process_near_pose_); + range_scan->SetCorrectedPose(range_scan->GetOdometricPose()); + process_near_pose_.reset(nullptr); + processed = smapper_->getMapper()->ProcessAgainstNodesNearBy( + range_scan, false, &covariance); + update_reprocessing_transform = true; + processor_type_ = PROCESS; + } else { + RCLCPP_FATAL( + get_logger(), "SlamToolbox: No valid processor type set! Exiting."); + exit(-1); + } + + // if successfully processed, create odom to map transformation + // and add our scan to storage + if (processed) { + if (enable_interactive_mode_) { + scan_holder_->addScan(*scan); + } + } else { + delete range_scan; + range_scan = nullptr; + } + + return range_scan; +} + +/*****************************************************************************/ +LaserRangeFinder * MultiRobotSlamToolbox::getLaser( + const slam_toolbox::msg::LocalizedLaserScan::ConstSharedPtr localized_scan) +/*****************************************************************************/ +{ + 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); + dataset_->Add(lasers_[frame].getLaser(), true); + } catch (tf2::TransformException & e) { + RCLCPP_ERROR( + get_logger(), "Failed to compute laser pose[%s], " + "aborting initialization (%s)", frame.c_str(), e.what()); + return nullptr; + } + } + + return lasers_[frame].getLaser(); +} + +/*****************************************************************************/ +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; + + scan_msg.scan = *scan; + + tf2::Quaternion q_offset(0., 0., 0., 1.0); + q_offset.setRPY(0., 0., offset.GetHeading()); + tf2::Transform scanner_offset(q_offset, tf2::Vector3(offset.GetX(), offset.GetY(), 0.0)); + tf2::toMsg(scanner_offset, scan_msg.scanner_offset.transform); + scan_msg.scanner_offset.header.stamp = t; + + tf2::Quaternion q(0., 0., 0., 1.0); + q.setRPY(0., 0., pose.GetHeading()); + tf2::Transform transform(q, tf2::Vector3(pose.GetX(), pose.GetY(), 0.0)); + tf2::toMsg(transform, scan_msg.pose.pose.pose); + + scan_msg.pose.pose.covariance[0] = cov(0, 0) * position_covariance_scale_; // x + scan_msg.pose.pose.covariance[1] = cov(0, 1) * position_covariance_scale_; // xy + scan_msg.pose.pose.covariance[6] = cov(1, 0) * position_covariance_scale_; // xy + scan_msg.pose.pose.covariance[7] = cov(1, 1) * position_covariance_scale_; // y + scan_msg.pose.pose.covariance[35] = cov(2, 2) * yaw_covariance_scale_; // yaw + scan_msg.pose.header.stamp = t; + + // Set prefixed frame names + 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()) == '/') ? + current_ns_ + map_frame_ : + current_ns_ + "/" + map_frame_; + + scan_msg.scanner_offset.child_frame_id = scan_msg.scan.header.frame_id; + + scan_msg.scanner_offset.header.frame_id = (*(base_frame_.cbegin()) == '/') ? + current_ns_ + base_frame_ : + current_ns_ + "/" + base_frame_; + + localized_scan_pub_->publish(scan_msg); +} + +/*****************************************************************************/ +bool MultiRobotSlamToolbox::deserializePoseGraphCallback( + const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr resp) +/*****************************************************************************/ +{ + if (req->match_type == procType::LOCALIZE_AT_POSE) { + RCLCPP_WARN( + get_logger(), "Requested a localization deserialization " + "in non-localization mode."); + return false; + } + + return SlamToolbox::deserializePoseGraphCallback(request_header, req, resp); +} + +} // namespace slam_toolbox diff --git a/src/slam_toolbox_multirobot_node.cpp b/src/slam_toolbox_multirobot_node.cpp new file mode 100644 index 000000000..c9ccf2eff --- /dev/null +++ b/src/slam_toolbox_multirobot_node.cpp @@ -0,0 +1,50 @@ +/* + * slam_toolbox + * Copyright Work Modifications (c) 2018, Simbe Robotics, Inc. + * Copyright Work Modifications (c) 2019, Steve Macenski + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#include +#include "slam_toolbox/slam_toolbox_multirobot.hpp" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + int stack_size = 40000000; + { + auto temp_node = std::make_shared("slam_toolbox"); + temp_node->declare_parameter("stack_size_to_use",rclcpp::ParameterType::PARAMETER_INTEGER ); + if (temp_node->get_parameter("stack_size_to_use", stack_size)) { + RCLCPP_INFO(temp_node->get_logger(), "Node using stack size %i", (int)stack_size); + const rlim_t max_stack_size = stack_size; + struct rlimit stack_limit; + getrlimit(RLIMIT_STACK, &stack_limit); + if (stack_limit.rlim_cur < stack_size) { + stack_limit.rlim_cur = stack_size; + } + setrlimit(RLIMIT_STACK, &stack_limit); + } + } + + rclcpp::NodeOptions options; + auto async_node = std::make_shared(options); + async_node->configure(); + async_node->loadPoseGraphByParams(); + rclcpp::spin(async_node->get_node_base_interface()); + rclcpp::shutdown(); + return 0; +}