diff --git a/CMakeLists.txt b/CMakeLists.txt index 63f50dc0c..a4034a873 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ project(slam_toolbox) add_compile_options(-std=c++11) set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/CMake/") -set(CMAKE_CXX_FLAGS "-fpermissive -std=c++11") +set(CMAKE_CXX_FLAGS "-fpermissive -std=c++11") find_package(Boost REQUIRED system serialization filesystem) @@ -80,6 +80,7 @@ add_service_files(DIRECTORY srvs LoopClosure.srv MergeMaps.srv AddSubmap.srv + AddMap.srv SerializePoseGraph.srv ) generate_messages( @@ -145,11 +146,11 @@ add_dependencies(slam_toolbox ${PROJECT_NAME}_generate_messages_cpp) target_link_libraries(slam_toolbox ${catkin_LIBRARIES} ${Boost_LIBRARIES}) #### Merging maps tool -add_executable(merge_maps_tool src/merge_maps_tool.cpp) -add_dependencies(merge_maps_tool ${PROJECT_NAME}_generate_messages_cpp) -target_link_libraries(merge_maps_tool ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +add_executable(merge_maps_kinematic src/merge_maps_kinematic.cpp) +add_dependencies(merge_maps_kinematic ${PROJECT_NAME}_generate_messages_cpp) +target_link_libraries(merge_maps_kinematic ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS slam_toolbox merge_maps_tool spa_solver_plugin g2O_solver_plugin ceres_solver_plugin SlamToolboxPlugin +install(TARGETS slam_toolbox merge_maps_kinematic spa_solver_plugin g2O_solver_plugin ceres_solver_plugin SlamToolboxPlugin RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} diff --git a/config/mapper_params.yaml b/config/mapper_params.yaml index db5fbec06..6341718b3 100644 --- a/config/mapper_params.yaml +++ b/config/mapper_params.yaml @@ -17,7 +17,8 @@ ceres_loss_function: None # ROS Parameters odom_frame: odom map_frame: map -base_frame: base_link +base_frame: base_footprint +laser_frame: base_laser_link debug_logging: false throttle_scans: 1 publish_occupancy_map: true @@ -63,3 +64,6 @@ coarse_angle_resolution: 0.0349 minimum_angle_penalty: 0.9 minimum_distance_penalty: 0.5 use_response_expansion: true + +# Load Map Parameters +inverted_laser: true \ No newline at end of file diff --git a/include/slam_toolbox/merge_maps_tool.hpp b/include/slam_toolbox/merge_maps_kinematic.hpp similarity index 74% rename from include/slam_toolbox/merge_maps_tool.hpp rename to include/slam_toolbox/merge_maps_kinematic.hpp index 1e443ab11..5f94f635a 100644 --- a/include/slam_toolbox/merge_maps_tool.hpp +++ b/include/slam_toolbox/merge_maps_kinematic.hpp @@ -48,36 +48,27 @@ #include #include -#include -#include -#include -#include -#include - #define MAP_IDX(sx, i, j) ((sx) * (j) + (i)) -class MergeMapTool +class MergeMapsKinematic { public: - MergeMapTool(); - ~MergeMapTool(); + MergeMapsKinematic(); + ~MergeMapsKinematic(); private: + // setup void SetConfigs(); - // callbacks + // callback bool MergeMapCallback(slam_toolbox::MergeMaps::Request &req, slam_toolbox::MergeMaps::Response &resp); bool AddSubmapCallback(slam_toolbox::AddSubmap::Request &req, slam_toolbox::AddSubmap::Response &resp); void ProcessInteractiveFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); - void KartoToROSOccupancyGrid(const karto::LocalizedRangeScanVector& scans); - - inline bool FileExists(const std::string& name) - { - struct stat buffer; - return (stat (name.c_str(), &buffer) == 0); - } + void KartoToROSOccupancyGrid(const karto::LocalizedRangeScanVector& scans, nav_msgs::GetMap::Response& map); + typedef std::vector::iterator localized_range_scans_vec_it; + typedef karto::LocalizedRangeScanVector::iterator localized_range_scans_it; // ROS-y-ness ros::NodeHandle nh_; @@ -89,7 +80,10 @@ class MergeMapTool double resolution_; double max_laser_range_; int num_submaps_; - nav_msgs::GetMap::Response map_; + + //karto bookkeeping + std::map lasers_; + std::vector dataset_vec_; // TF tf::TransformBroadcaster* tfB_; @@ -97,4 +91,10 @@ class MergeMapTool // visualization interactive_markers::InteractiveMarkerServer* interactive_server_; std::map submap_locations_; + std::vector scans_vec_; + std::map submap_marker_transform_; + + //apply transformation to correct pose + karto::Pose2 ApplyCorrection(const karto::Pose2& pose, const tf::Transform& submap_correction); + karto::Vector2 ApplyCorrection(const karto::Vector2& pose, const tf::Transform& submap_correction); }; diff --git a/include/slam_toolbox/slam_toolbox.hpp b/include/slam_toolbox/slam_toolbox.hpp index 326714ba8..2015005a2 100644 --- a/include/slam_toolbox/slam_toolbox.hpp +++ b/include/slam_toolbox/slam_toolbox.hpp @@ -50,6 +50,7 @@ #include "slam_toolbox/SaveMap.h" #include "slam_toolbox/LoopClosure.h" #include "slam_toolbox/SerializePoseGraph.h" +#include "slam_toolbox/AddMap.h" #include #include @@ -135,6 +136,8 @@ class SlamToolbox // state bool IsPaused(const PausedApplication& app); + bool LoadMapperCallback(slam_toolbox::AddMap::Request &req, slam_toolbox::AddMap::Response &resp); + // ROS-y-ness ros::NodeHandle nh_; tf::TransformListener tf_; @@ -142,11 +145,11 @@ class SlamToolbox message_filters::Subscriber* scan_filter_sub_; tf::MessageFilter* scan_filter_; ros::Publisher sst_, sstm_, marker_publisher_, scan_publisher_; - ros::ServiceServer ssMap_, ssClear_, ssInteractive_, ssLoopClosure_, ssPause_processing_, ssPause_measurements_, ssClear_manual_, ssSave_map_, ssSerialize_; + ros::ServiceServer ssMap_, ssClear_, ssInteractive_, ssLoopClosure_, ssPause_processing_, ssPause_measurements_, ssClear_manual_, ssSave_map_, ssSerialize_, ssLoadMap_; nav_msgs::GetMap::Response map_; // Storage for ROS parameters - std::string odom_frame_, map_frame_, base_frame_; + std::string odom_frame_, map_frame_, base_frame_, laser_frame_; int throttle_scans_; ros::Duration map_update_interval_; double resolution_, minimum_time_interval_, minimum_travel_distance_; diff --git a/launch/build_map_w_params.launch b/launch/build_map_w_params.launch index ed37bf6ba..406feac7c 100644 --- a/launch/build_map_w_params.launch +++ b/launch/build_map_w_params.launch @@ -1,7 +1,5 @@ - - - - - + + + \ No newline at end of file diff --git a/launch/merge_maps_kinematic.launch b/launch/merge_maps_kinematic.launch new file mode 100644 index 000000000..a48768a9c --- /dev/null +++ b/launch/merge_maps_kinematic.launch @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/launch/merge_maps_tool.launch b/launch/merge_maps_tool.launch deleted file mode 100644 index caf4c7953..000000000 --- a/launch/merge_maps_tool.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/mapping_plugin/slam_toolbox_rviz_plugin.cpp b/mapping_plugin/slam_toolbox_rviz_plugin.cpp index 63c98a1b1..3dcb5862f 100644 --- a/mapping_plugin/slam_toolbox_rviz_plugin.cpp +++ b/mapping_plugin/slam_toolbox_rviz_plugin.cpp @@ -43,7 +43,8 @@ SlamToolboxPlugin::SlamToolboxPlugin(QWidget* parent): nh.getParam("/slam_toolbox/paused_new_measurements", paused_measure); nh.getParam("/slam_toolbox/paused_processing", paused_process); nh.getParam("/slam_toolbox/interactive_mode", interactive); - + _serialize = nh.serviceClient("/slam_toolbox/serialize_map"); + _load_map = nh.serviceClient("/slam_toolbox/load_map"); _clearChanges = nh.serviceClient("/slam_toolbox/clear_changes"); _saveChanges = nh.serviceClient("/slam_toolbox/manual_loop_closure"); _saveMap = nh.serviceClient("/slam_toolbox/save_map"); @@ -51,9 +52,8 @@ SlamToolboxPlugin::SlamToolboxPlugin(QWidget* parent): _interactive = nh.serviceClient("/slam_toolbox/toggle_interactive_mode"); _pause_processing = nh.serviceClient("/slam_toolbox/pause_processing"); _pause_measurements = nh.serviceClient("/slam_toolbox/pause_new_measurements"); - _load_submap = nh.serviceClient("/merge_maps_tool/add_submap"); - _merge = nh.serviceClient("/merge_maps_tool/merge_maps"); - _serialize = nh.serviceClient("/slam_toolbox/serialize_map"); + _load_submap_for_merging = nh.serviceClient("/map_merging/add_submap"); + _merge = nh.serviceClient("/map_merging/merge_submaps"); _vbox = new QVBoxLayout(); _hbox1 = new QHBoxLayout(); @@ -63,6 +63,7 @@ SlamToolboxPlugin::SlamToolboxPlugin(QWidget* parent): _hbox5 = new QHBoxLayout(); _hbox6 = new QHBoxLayout(); _hbox7 = new QHBoxLayout(); + _hbox8 = new QHBoxLayout(); QFrame* _line = new QFrame(); _line->setFrameShape(QFrame::HLine); @@ -81,14 +82,17 @@ SlamToolboxPlugin::SlamToolboxPlugin(QWidget* parent): _button4->setText("Clear Measurement Queue"); connect(_button4, SIGNAL(clicked()), this, SLOT(ClearQueue())); _button5 = new QPushButton(this); - _button5->setText("Load Pose Graph"); - connect(_button5, SIGNAL(clicked()), this, SLOT(LoadPoseGraph())); + _button5->setText("Add Submap"); + connect(_button5, SIGNAL(clicked()), this, SLOT(LoadSubmap())); _button6 = new QPushButton(this); _button6->setText("Generate Map"); connect(_button6, SIGNAL(clicked()), this, SLOT(GenerateMap())); _button7 = new QPushButton(this); _button7->setText("Serialize Map"); connect(_button7, SIGNAL(clicked()), this, SLOT(SerializeMap())); + _button8 = new QPushButton(this); + _button8->setText("Load Map"); + connect(_button8, SIGNAL(clicked()), this, SLOT(LoadMap())); _label1 = new QLabel(this); _label1->setText("Interactive"); @@ -116,6 +120,7 @@ SlamToolboxPlugin::SlamToolboxPlugin(QWidget* parent): _line1 = new QLineEdit(); _line2 = new QLineEdit(); _line3 = new QLineEdit(); + _line4 = new QLineEdit(); _button1->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); _button2->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); @@ -133,6 +138,7 @@ SlamToolboxPlugin::SlamToolboxPlugin(QWidget* parent): _hbox1->addWidget(_check3); _hbox1->addWidget(_label3); + _hbox2->addWidget(_button1); _hbox2->addWidget(_button2); @@ -149,11 +155,15 @@ SlamToolboxPlugin::SlamToolboxPlugin(QWidget* parent): _hbox7->addWidget(_button7); _hbox7->addWidget(_line3); + _hbox8->addWidget(_button8); + _hbox8->addWidget(_line4); + _vbox->addWidget(_label5); _vbox->addLayout(_hbox1); _vbox->addLayout(_hbox2); _vbox->addLayout(_hbox3); _vbox->addLayout(_hbox7); + _vbox->addLayout(_hbox8); _vbox->addLayout(_hbox4); _vbox->addWidget(_line); _vbox->addWidget(_label4); @@ -183,22 +193,33 @@ void SlamToolboxPlugin::SerializeMap() msg.request.filename = _line3->text().toStdString(); if (!_serialize.call(msg)) { - ROS_WARN("Failed to serialize pose graph to file, is service running?"); + ROS_WARN("SlamToolbox: Failed to serialize pose graph to file, is service running?"); } } /*****************************************************************************/ -void SlamToolboxPlugin::LoadPoseGraph() +void SlamToolboxPlugin::LoadMap() /*****************************************************************************/ { slam_toolbox::AddSubmap msg; - msg.request.filename = _line2->text().toStdString(); - if (!_load_submap.call(msg)) + msg.request.filename = _line4->text().toStdString(); + if (!_load_map.call(msg)) { - ROS_WARN("Failed to load pose graph from file, is service running?"); + ROS_WARN("SlamToolbox: Failed to load mapper object from file, is service running?"); } } +/*****************************************************************************/ +void SlamToolboxPlugin::LoadSubmap() +/*****************************************************************************/ +{ + slam_toolbox::AddSubmap msg; + msg.request.filename = _line2->text().toStdString(); + if (!_load_submap_for_merging.call(msg)) + { + ROS_WARN("MergeMaps: Failed to load pose graph from file, is service running?"); + } +} /*****************************************************************************/ void SlamToolboxPlugin::GenerateMap() /*****************************************************************************/ @@ -206,7 +227,7 @@ void SlamToolboxPlugin::GenerateMap() slam_toolbox::MergeMaps msg; if (!_merge.call(msg)) { - ROS_WARN("Failed to merge maps, is service running?"); + ROS_WARN("MergeMaps: Failed to merge maps, is service running?"); } } @@ -217,7 +238,7 @@ void SlamToolboxPlugin::ClearChanges() slam_toolbox::Clear msg; if (!_clearChanges.call(msg)) { - ROS_WARN("Failed to clear changes, is service running?"); + ROS_WARN("SlamToolbox: Failed to clear changes, is service running?"); } } @@ -229,7 +250,7 @@ void SlamToolboxPlugin::SaveChanges() if (!_saveChanges.call(msg)) { - ROS_WARN("Failed to save changes, is service running?"); + ROS_WARN("SlamToolbox: Failed to save changes, is service running?"); } } @@ -241,7 +262,7 @@ void SlamToolboxPlugin::SaveMap() msg.request.name.data = _line1->text().toStdString(); if (!_saveMap.call(msg)) { - ROS_WARN("Failed to save map as %s, is service running?", + ROS_WARN("SlamToolbox: Failed to save map as %s, is service running?", msg.request.name.data.c_str()); } } @@ -264,7 +285,7 @@ void SlamToolboxPlugin::InteractiveCb(int state) slam_toolbox::ToggleInteractive msg; if (!_interactive.call(msg)) { - ROS_WARN("Failed to toggle interactive mode, is service running?"); + ROS_WARN("SlamToolbox: Failed to toggle interactive mode, is service running?"); } } @@ -275,7 +296,7 @@ void SlamToolboxPlugin::PauseProcessingCb(int state) slam_toolbox::Pause msg; if (!_pause_processing.call(msg)) { - ROS_WARN("Failed to toggle pause processing, is service running?"); + ROS_WARN("SlamToolbox: Failed to toggle pause processing, is service running?"); } } @@ -286,7 +307,7 @@ void SlamToolboxPlugin::PauseMeasurementsCb(int state) slam_toolbox::Pause msg; if (!_pause_measurements.call(msg)) { - ROS_WARN("Failed to toggle pause measurements, is service running?"); + ROS_WARN("SlamToolbox: Failed to toggle pause measurements, is service running?"); } } diff --git a/mapping_plugin/slam_toolbox_rviz_plugin.hpp b/mapping_plugin/slam_toolbox_rviz_plugin.hpp index 08b766ee4..bb581806c 100644 --- a/mapping_plugin/slam_toolbox_rviz_plugin.hpp +++ b/mapping_plugin/slam_toolbox_rviz_plugin.hpp @@ -47,6 +47,7 @@ #include "slam_toolbox/LoopClosure.h" #include "slam_toolbox/MergeMaps.h" #include "slam_toolbox/AddSubmap.h" +#include "slam_toolbox/AddMap.h" #include "slam_toolbox/SerializePoseGraph.h" class QLineEdit; @@ -75,9 +76,10 @@ protected Q_SLOTS: void InteractiveCb(int state); void PauseProcessingCb(int state); void PauseMeasurementsCb(int state); - void LoadPoseGraph(); + void LoadSubmap(); void GenerateMap(); void SerializeMap(); + void LoadMap(); void updateCheckStateIfExternalChange(); @@ -90,6 +92,7 @@ protected Q_SLOTS: QHBoxLayout* _hbox5; QHBoxLayout* _hbox6; QHBoxLayout* _hbox7; + QHBoxLayout* _hbox8; QPushButton* _button1; QPushButton* _button2; @@ -98,10 +101,12 @@ protected Q_SLOTS: QPushButton* _button5; QPushButton* _button6; QPushButton* _button7; + QPushButton* _button8; QLineEdit* _line1; QLineEdit* _line2; QLineEdit* _line3; + QLineEdit* _line4; QCheckBox* _check1; QCheckBox* _check2; @@ -115,7 +120,7 @@ protected Q_SLOTS: QFrame* _line; - ros::ServiceClient _clearChanges, _saveChanges, _saveMap, _clearQueue, _interactive, _pause_processing, _pause_measurements, _load_submap, _merge, _serialize; + ros::ServiceClient _clearChanges, _saveChanges, _saveMap, _clearQueue, _interactive, _pause_processing, _pause_measurements, _load_submap_for_merging, _merge, _serialize, _load_map; std::thread* _thread; }; diff --git a/src/merge_maps_kinematic.cpp b/src/merge_maps_kinematic.cpp new file mode 100644 index 000000000..9c7c52ad0 --- /dev/null +++ b/src/merge_maps_kinematic.cpp @@ -0,0 +1,413 @@ +/* + * Author + * Copyright (c) 2018, Simbe Robotics, Inc. + * + * 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 "serialization.cpp" + +/*****************************************************************************/ +MergeMapsKinematic::MergeMapsKinematic() : interactive_server_(NULL) +/*****************************************************************************/ +{ + ROS_INFO("MergeMapsKinematic: Starting up!"); + interactive_server_ = \ + new interactive_markers::InteractiveMarkerServer("merge_maps_tool","",true); + + ros::NodeHandle nh_tmp("~"); + nh_ = nh_tmp; + SetConfigs(); +} + +/*****************************************************************************/ +void MergeMapsKinematic::SetConfigs() +/*****************************************************************************/ +{ + if(!nh_.getParam("map_frame", map_frame_)) + map_frame_ = "map"; + if(!nh_.getParam("max_laser_range", max_laser_range_)) + max_laser_range_ = 25.0; + if(!nh_.getParam("resolution", resolution_)) + { + resolution_ = 0.05; + } + sstS_.push_back(nh_.advertise("/map", 1, true)); + sstmS_.push_back(nh_.advertise( \ + "/map_metadata", 1, true)); + ros::NodeHandle nh("map_merging"); + ssMap_ = nh.advertiseService("merge_submaps", + &MergeMapsKinematic::MergeMapCallback, this); + ssSubmap_ = nh.advertiseService("add_submap", + &MergeMapsKinematic::AddSubmapCallback, this); + num_submaps_ = 0; + tfB_ = new tf::TransformBroadcaster(); +} + +/*****************************************************************************/ +MergeMapsKinematic::~MergeMapsKinematic() +/*****************************************************************************/ +{ + if (interactive_server_) + { + delete interactive_server_; + } + for(std::vector::iterator dataset_it = dataset_vec_.begin(); + dataset_it != dataset_vec_.end(); ++dataset_it ) + { + delete *dataset_it; + } +} + +/*****************************************************************************/ +bool MergeMapsKinematic::AddSubmapCallback(slam_toolbox::AddSubmap::Request &req, + slam_toolbox::AddSubmap::Response &resp) +/*****************************************************************************/ +{ + karto::Mapper* mapper = new karto::Mapper; + karto::Dataset* dataset = new karto::Dataset; + serialization::Read(req.filename, mapper, dataset); + karto::LaserRangeFinder* laser = dynamic_cast\ + (dataset->GetObjects()[0]); + if (lasers_.find(laser->GetName().GetName())==lasers_.end()) + { + lasers_[laser->GetName().GetName()] = laser; + dataset->Add(laser); + dataset_vec_.push_back(dataset); + } + + karto::LocalizedRangeScanVector scans = mapper->GetAllProcessedScans(); + scans_vec_.push_back(scans); + num_submaps_++; + + // create and publish map with marker that will move the map around + sstS_.push_back(nh_.advertise( \ + "/map_"+std::to_string(num_submaps_), 1, true)); + sstmS_.push_back(nh_.advertise( \ + "/map_metadata_" + std::to_string(num_submaps_), 1, true)); + ros::Duration(1.).sleep(); + nav_msgs::GetMap::Response map; + KartoToROSOccupancyGrid(scans,map); + + tf::Transform transform; + transform.setIdentity(); + transform.setOrigin(tf::Vector3(map.map.info.origin.position.x + + map.map.info.width * map.map.info.resolution / 2.0,\ + map.map.info.origin.position.y + + map.map.info.height * map.map.info.resolution / 2.0,\ + 0.)); + map.map.info.origin.position.x = - (map.map.info.width * map.map.info.resolution / 2.0); + map.map.info.origin.position.y = - (map.map.info.height * map.map.info.resolution / 2.0); + map.map.header.stamp = ros::Time::now(); + map.map.header.frame_id = "map_"+std::to_string(num_submaps_); + sstS_[num_submaps_].publish(map.map); + sstmS_[num_submaps_].publish(map.map.info); + tfB_->sendTransform(tf::StampedTransform (transform, ros::Time::now(), + "/map", "/map_"+std::to_string(num_submaps_))); + submap_marker_transform_[num_submaps_]=tf::Transform(tf::createQuaternionFromRPY(0,0,0), + tf::Vector3(0,0,0));//no initial correction -- identity mat + + // create an interactive marker for the base of this frame and attach it + visualization_msgs::Marker m; + m.header.frame_id = "map"; + m.header.stamp = ros::Time::now(); + m.ns = "merge_maps_tool"; + m.type = visualization_msgs::Marker::SPHERE; + m.pose.position.z = 0.0; + m.pose.position.x = transform.getOrigin().getX(); + m.pose.position.y = transform.getOrigin().getY(); + submap_locations_[num_submaps_] = Eigen::Vector3d(transform.getOrigin().getX(), + transform.getOrigin().getY(),0.); + m.pose.orientation.w = 1.; + m.scale.x = 2; m.scale.y = 2; m.scale.z = 2; + m.color.r = 1.0; m.color.g = 0; m.color.b = 0.0; m.color.a = 1.; + m.action = visualization_msgs::Marker::ADD; + m.lifetime = ros::Duration(0.); + m.id = num_submaps_; + + // marker and metadata + visualization_msgs::InteractiveMarker int_marker; + int_marker.header.frame_id = "map"; + int_marker.header.stamp = ros::Time::now(); + int_marker.name = std::to_string(num_submaps_); + int_marker.pose.orientation.w = 1.; + int_marker.pose.position.x = m.pose.position.x; + int_marker.pose.position.y = m.pose.position.y; + int_marker.scale = 2.4; + + // translate control + visualization_msgs::InteractiveMarkerControl control; + control.orientation_mode = \ + visualization_msgs::InteractiveMarkerControl::FIXED; + control.always_visible = true; + control.orientation.w = 0; + control.orientation.x = 0.7071; + control.orientation.y = 0; + control.orientation.z = 0.7071; + control.interaction_mode = \ + visualization_msgs::InteractiveMarkerControl::MOVE_PLANE; + control.markers.push_back( m ); + int_marker.controls.push_back( control ); + + // rotate control + visualization_msgs::InteractiveMarkerControl control_rot; + control_rot.orientation_mode = \ + visualization_msgs::InteractiveMarkerControl::FIXED; + control_rot.always_visible = true; + control_rot.orientation.w = 0; + control_rot.orientation.x = 0.7071; + control_rot.orientation.y = 0; + control_rot.orientation.z = 0.7071; + control_rot.interaction_mode = \ + visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; + int_marker.controls.push_back( control_rot ); + + interactive_server_->insert(int_marker, \ + boost::bind(&MergeMapsKinematic::ProcessInteractiveFeedback, this, _1)); + interactive_server_->applyChanges(); + delete mapper; + mapper = NULL; + return true; +} + +/*****************************************************************************/ +karto::Pose2 MergeMapsKinematic::ApplyCorrection(const \ + karto::Pose2& pose, + const tf::Transform& submap_correction) +/*****************************************************************************/ +{ + tf::Transform pose_tf, pose_corr; + pose_tf.setOrigin(tf::Vector3(pose.GetX(), pose.GetY(), 0.)); + pose_tf.setRotation(tf::createQuaternionFromRPY(0, 0, pose.GetHeading())); + pose_corr = submap_correction * pose_tf; + return karto::Pose2(pose_corr.getOrigin().x(), pose_corr.getOrigin().y(),\ + tf::getYaw(pose_corr.getRotation())); +} + +/*****************************************************************************/ +karto::Vector2 MergeMapsKinematic::ApplyCorrection(const \ + karto::Vector2& pose, + const tf::Transform& submap_correction) +/*****************************************************************************/ +{ + tf::Transform pose_tf, pose_corr; + pose_tf.setOrigin(tf::Vector3(pose.GetX(), pose.GetY(), 0.)); + pose_tf.setRotation(tf::createQuaternionFromRPY(0, 0, 0)); + pose_corr = submap_correction * pose_tf; + return karto::Vector2(pose_corr.getOrigin().x(), pose_corr.getOrigin().y()); +} + +/*****************************************************************************/ +bool MergeMapsKinematic::MergeMapCallback(slam_toolbox::MergeMaps::Request &req, + slam_toolbox::MergeMaps::Response &resp) +/*****************************************************************************/ +{ + int id = 0; + karto::Pose2 corrected_pose; + karto::LocalizedRangeScan* pScan; + karto::LocalizedRangeScanVector transformed_scans; + std::vector& vector_of_scans = scans_vec_; + karto::LocalizedRangeScan* pScan_copy; + + for(localized_range_scans_vec_it it_LRV = vector_of_scans.begin(); + it_LRV!= vector_of_scans.end(); ++it_LRV) + { + id++; + for ( localized_range_scans_it iter = (*it_LRV).begin(); + iter != (*it_LRV).end();++iter) + { + pScan= *iter; + pScan_copy = pScan; + tf::Transform submap_correction = submap_marker_transform_[id]; + + //TRANSFORM BARYCENTERR POSE + const karto::Pose2 baryCenter_pose = pScan_copy->GetBarycenterPose(); + karto::Pose2 karto_baryCenterPose_corr = ApplyCorrection(baryCenter_pose, submap_correction); + pScan_copy->SetBarycenterPose(karto_baryCenterPose_corr); + + //TRANSFORM BOUNDING BOX POSITIONS + karto::BoundingBox2 bbox = pScan_copy->GetBoundingBox(); + const karto::Vector2 bbox_min_corr = ApplyCorrection(bbox.GetMinimum(),\ + submap_correction); + bbox.SetMinimum(bbox_min_corr); + const karto::Vector2 bbox_max_corr = ApplyCorrection(bbox.GetMaximum(),\ + submap_correction); + bbox.SetMaximum(bbox_max_corr); + pScan_copy->SetBoundingBox(bbox); + + // TRANSFORM UNFILTERED POINTS USED + karto::PointVectorDouble UPR_vec = pScan_copy->GetPointReadings(); + for(karto::PointVectorDouble::iterator it_upr = UPR_vec.begin(); + it_upr!=UPR_vec.end(); ++it_upr) + { + const karto::Vector2 upr_corr = ApplyCorrection(*it_upr, submap_correction); + (*it_upr).SetX(upr_corr.GetX()); + (*it_upr).SetY(upr_corr.GetY()); + } + pScan_copy->SetPointReadings(UPR_vec); + + //TRANSFORM CORRECTED POSE + corrected_pose = pScan_copy->GetCorrectedPose(); + karto::Pose2 karto_robotPose_corr = ApplyCorrection(corrected_pose, submap_correction); + pScan_copy->SetCorrectedPose(karto_robotPose_corr); + kt_bool rIsDirty = false; + pScan_copy->SetIsDirty(rIsDirty); + + //TRANSFORM ODOM POSE + karto::Pose2 odom_pose = pScan_copy->GetOdometricPose(); + karto::Pose2 karto_robotPose_odom = ApplyCorrection(odom_pose, submap_correction); + pScan_copy->SetOdometricPose(karto_robotPose_odom); + transformed_scans.push_back(pScan_copy); + } + } + nav_msgs::GetMap::Response map; + KartoToROSOccupancyGrid(transformed_scans, map); + map.map.header.stamp = ros::Time::now(); + map.map.header.frame_id = "map"; + sstS_[0].publish(map.map); + sstmS_[0].publish(map.map.info); +} +/*****************************************************************************/ +void MergeMapsKinematic::KartoToROSOccupancyGrid( \ + const karto::LocalizedRangeScanVector& scans, + nav_msgs::GetMap::Response& map) +/*****************************************************************************/ +{ + karto::OccupancyGrid* occ_grid = NULL; + occ_grid = karto::OccupancyGrid::CreateFromScans(scans, resolution_); + if (!occ_grid) + { + ROS_INFO("MergeMapsKinematic: Could not make Karto occupancy grid."); + delete occ_grid; + return; + } + // Translate to ROS format + kt_int32s width = occ_grid->GetWidth(); + kt_int32s height = occ_grid->GetHeight(); + karto::Vector2 offset = \ + occ_grid->GetCoordinateConverter()->GetOffset(); + + if(map.map.info.width != (unsigned int) width || + map.map.info.height != (unsigned int) height || + map.map.info.origin.position.x != offset.GetX() || + map.map.info.origin.position.y != offset.GetY()) + { + map.map.info.origin.position.x = offset.GetX(); + map.map.info.origin.position.y = offset.GetY(); + map.map.info.origin.position.z = 0.0; + map.map.info.origin.orientation.w = 1.0; + map.map.info.width = width; + map.map.info.height = height; + map.map.info.resolution = resolution_; + map.map.data.resize(map.map.info.width * map.map.info.height); + } + + for (kt_int32s y=0; yGetValue(karto::Vector2(x, y)); + switch (value) + { + case karto::GridStates_Unknown: + map.map.data[MAP_IDX(map.map.info.width, x, y)] = -1; + break; + case karto::GridStates_Occupied: + map.map.data[MAP_IDX(map.map.info.width, x, y)] = 100; + break; + case karto::GridStates_Free: + map.map.data[MAP_IDX(map.map.info.width, x, y)] = 0; + break; + default: + ROS_WARN("Encountered unknown cell value at %d, %d", x, y); + break; + } + } + } + delete occ_grid; + return; +} + +/*****************************************************************************/ +void MergeMapsKinematic::ProcessInteractiveFeedback(const \ + visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) +/*****************************************************************************/ +{ + if (feedback->event_type == \ + visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP && + feedback->mouse_point_valid) + { + // we offset by 1 + const int id = std::stoi(feedback->marker_name,nullptr,10); + + // get yaw + tfScalar yaw, pitch, roll; + tf::Quaternion quat; + tf::quaternionMsgToTF(feedback->pose.orientation, quat); // relative + tf::Matrix3x3 mat(quat); + mat.getRPY(roll, pitch, yaw); + tf::Transform previous_submap_correction ; + previous_submap_correction.setIdentity(); + previous_submap_correction.setOrigin(tf::Vector3(submap_locations_[id](0), + submap_locations_[id](1), 0.)); + + // update internal knowledge of submap locations + submap_locations_[id] = Eigen::Vector3d(feedback->pose.position.x, \ + feedback->pose.position.y, \ + submap_locations_[id](2) + yaw); + + // add the map_N frame there + tf::Transform new_submap_location; + new_submap_location.setOrigin(tf::Vector3(submap_locations_[id](0), \ + submap_locations_[id](1), 0.)); + new_submap_location.setRotation(quat); + tfB_->sendTransform(tf::StampedTransform (new_submap_location, + ros::Time::now(), "/map", "/map_"+std::to_string(id))); + + submap_marker_transform_[id] = submap_marker_transform_[id] *\ + previous_submap_correction.inverse() * new_submap_location; + } + + if (feedback->event_type == \ + visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE) + { + const int id = std::stoi(feedback->marker_name,nullptr,10); + + // get yaw + tfScalar yaw, pitch, roll; + tf::Quaternion quat; + tf::quaternionMsgToTF(feedback->pose.orientation, quat); // relative + tf::Matrix3x3 mat(quat); + mat.getRPY(roll, pitch, yaw); + + // add the map_N frame there + tf::Transform new_submap_location; + new_submap_location.setOrigin(tf::Vector3(feedback->pose.position.x, \ + feedback->pose.position.y, 0.)); + new_submap_location.setRotation(quat); + tfB_->sendTransform(tf::StampedTransform (new_submap_location, + ros::Time::now(), "/map", "/map_"+std::to_string(id))); + } +} + +/*****************************************************************************/ +int main(int argc, char** argv) +/*****************************************************************************/ +{ + ros::init(argc, argv, "merge_maps_kinematic"); + MergeMapsKinematic mmk; + ros::spin(); + return 0; +} diff --git a/src/merge_maps_tool.cpp b/src/merge_maps_tool.cpp deleted file mode 100644 index d0a23dc1d..000000000 --- a/src/merge_maps_tool.cpp +++ /dev/null @@ -1,304 +0,0 @@ -/* - * Author - * Copyright (c) 2018, Simbe Robotics, Inc. - * - * 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 "serialization.cpp" - -/*****************************************************************************/ -MergeMapTool::MergeMapTool() : interactive_server_(NULL) -/*****************************************************************************/ -{ - ROS_INFO("MergeMapTool: Starting up!"); - interactive_server_ = \ - new interactive_markers::InteractiveMarkerServer("merge_maps_tool","",true); - - ros::NodeHandle nh_tmp("~"); - nh_ = nh_tmp; - SetConfigs(); -} - -/*****************************************************************************/ -void MergeMapTool::SetConfigs() -/*****************************************************************************/ -{ - if(!nh_.getParam("map_frame", map_frame_)) - map_frame_ = "map"; - if(!nh_.getParam("max_laser_range", max_laser_range_)) - max_laser_range_ = 25.0; - if(!nh_.getParam("resolution", resolution_)) - { - resolution_ = 0.05; - } - - sstS_.push_back(nh_.advertise("/map", 1, true)); - sstmS_.push_back(nh_.advertise( \ - "/map_metadata", 1, true)); - ssMap_ = nh_.advertiseService("merge_maps", - &MergeMapTool::MergeMapCallback, this); - ssSubmap_ = nh_.advertiseService("add_submap", - &MergeMapTool::AddSubmapCallback, this); - num_submaps_ = 0; - tfB_ = new tf::TransformBroadcaster(); -} - -/*****************************************************************************/ -MergeMapTool::~MergeMapTool() -/*****************************************************************************/ -{ - if (interactive_server_) - { - delete interactive_server_; - } -} - -/*****************************************************************************/ -bool MergeMapTool::AddSubmapCallback(slam_toolbox::AddSubmap::Request &req, - slam_toolbox::AddSubmap::Response &resp) -/*****************************************************************************/ -{ - // find if file exists - const std::string filename = req.filename + std::string(".st"); - if (!FileExists(filename)) - { - ROS_ERROR("MergeMapTool: Failed to open requested submap %s.", filename.c_str()); - return true; - } - - karto::Mapper* mapper = new karto::Mapper(); - serialization::Read(filename, mapper); - karto::LocalizedRangeScanVector scans = mapper->GetAllProcessedScans(); // TODO should I be saving a vect or of these mapper objects? / scans, A: YES - - // num_submaps_++ and name frame appropriately - num_submaps_++; - submap_locations_[num_submaps_] = Eigen::Vector3d(0.,0.,0.); - - // create and publish map with marker that will move the map around - sstS_.push_back(nh_.advertise( \ - "/map_"+std::to_string(num_submaps_), 1, true)); - sstmS_.push_back(nh_.advertise( \ - "/map_metadata_" + std::to_string(num_submaps_), 1, true)); - ros::Duration(1.).sleep(); - - KartoToROSOccupancyGrid(scans); - map_.map.header.stamp = ros::Time::now(); - map_.map.header.frame_id = "map_"+std::to_string(num_submaps_); - sstS_[num_submaps_].publish(map_.map); - sstmS_[num_submaps_].publish(map_.map.info); - - // create an interactive marker for the base of this frame and attach it - visualization_msgs::Marker m; - m.header.frame_id = "map"; - m.header.stamp = ros::Time::now(); - m.ns = "merge_maps_tool"; - m.type = visualization_msgs::Marker::SPHERE; - m.pose.position.z = 0.0; - m.pose.orientation.w = 1.; - m.scale.x = 0.4; m.scale.y = 0.4; m.scale.z = 0.4; - m.color.r = 1.0; m.color.g = 0; m.color.b = 0.0; m.color.a = 1.; - m.action = visualization_msgs::Marker::ADD; - m.lifetime = ros::Duration(0.); - - m.id = num_submaps_; - m.pose.position.x = 0.; - m.pose.position.y = 0.; - - // marker and metadata - visualization_msgs::InteractiveMarker int_marker; - int_marker.header.frame_id = "map"; - int_marker.header.stamp = ros::Time::now(); - int_marker.name = std::to_string(num_submaps_); - int_marker.pose.orientation.w = 1.; - int_marker.pose.position.x = m.pose.position.x; - int_marker.pose.position.y = m.pose.position.y; - int_marker.scale = 0.6; - - // translate control - visualization_msgs::InteractiveMarkerControl control; - control.orientation_mode = \ - visualization_msgs::InteractiveMarkerControl::FIXED; - control.always_visible = true; - control.orientation.w = 0; - control.orientation.x = 0.7071; - control.orientation.y = 0; - control.orientation.z = 0.7071; - control.interaction_mode = \ - visualization_msgs::InteractiveMarkerControl::MOVE_PLANE; - control.markers.push_back( m ); - int_marker.controls.push_back( control ); - - // rotate control - visualization_msgs::InteractiveMarkerControl control_rot; - control_rot.orientation_mode = \ - visualization_msgs::InteractiveMarkerControl::FIXED; - control_rot.always_visible = true; - control_rot.orientation.w = 0; - control_rot.orientation.x = 0.7071; - control_rot.orientation.y = 0; - control_rot.orientation.z = 0.7071; - control_rot.interaction_mode = \ - visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; - int_marker.controls.push_back( control_rot ); - - interactive_server_->insert(int_marker, \ - boost::bind(&MergeMapTool::ProcessInteractiveFeedback, this, _1)); - interactive_server_->applyChanges(); - - return true; -} - -/*****************************************************************************/ -bool MergeMapTool::MergeMapCallback(slam_toolbox::MergeMaps::Request &req, - slam_toolbox::MergeMaps::Response &resp) -/*****************************************************************************/ -{ - //TODO - // take submaps and project all positions into map frame,submap_locations_ - // scans - - // create map using kartooccupany grid from all in same frame - // KartoToROSOccupancyGrid(scans) - - // publish it for visualization/map_saver using sstMS_[0]'s for meta and normal' - - // later, take nodes in graph mutually closest to each other and scana match - // against them to make a new inter-graph contraint list to optimize over - // then generate new composite map - // (this same technique can then be applied with manual loop closures) -} - -/*****************************************************************************/ -void MergeMapTool::KartoToROSOccupancyGrid( \ - const karto::LocalizedRangeScanVector& scans) -/*****************************************************************************/ -{ - karto::OccupancyGrid* occ_grid = NULL; - occ_grid = karto::OccupancyGrid::CreateFromScans(scans, resolution_); - - if (!occ_grid) - { - ROS_INFO("MergeMapTool: Could not make Karto occupancy grid."); - return; - } - - // Translate to ROS format - kt_int32s width = occ_grid->GetWidth(); - kt_int32s height = occ_grid->GetHeight(); - karto::Vector2 offset = \ - occ_grid->GetCoordinateConverter()->GetOffset(); - - if(map_.map.info.width != (unsigned int) width || - map_.map.info.height != (unsigned int) height || - map_.map.info.origin.position.x != offset.GetX() || - map_.map.info.origin.position.y != offset.GetY()) - { - map_.map.info.origin.position.x = offset.GetX(); - map_.map.info.origin.position.y = offset.GetY(); - map_.map.info.width = width; - map_.map.info.height = height; - map_.map.data.resize(map_.map.info.width * map_.map.info.height); - } - - for (kt_int32s y=0; yGetValue(karto::Vector2(x, y)); - switch (value) - { - case karto::GridStates_Unknown: - map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1; - break; - case karto::GridStates_Occupied: - map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100; - break; - case karto::GridStates_Free: - map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0; - break; - default: - ROS_WARN("Encountered unknown cell value at %d, %d", x, y); - break; - } - } - } - return; -} - -/*****************************************************************************/ -void MergeMapTool::ProcessInteractiveFeedback(const \ - visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) -/*****************************************************************************/ -{ - if (feedback->event_type == \ - visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP && - feedback->mouse_point_valid) - { - // we offset by 1 - const int id = std::stoi(feedback->marker_name,nullptr,10); - - // get yaw - tfScalar yaw, pitch, roll; - tf::Quaternion quat; - tf::quaternionMsgToTF(feedback->pose.orientation, quat); // relative - tf::Matrix3x3 mat(quat); - mat.getRPY(roll, pitch, yaw); - - // update internal knowledge of submap locations - submap_locations_[id] = Eigen::Vector3d(feedback->pose.position.x, \ - feedback->pose.position.y, \ - submap_locations_[id](2) + yaw); - - // add the map_N frame there - tf::Transform transform; - transform.setOrigin(tf::Vector3(submap_locations_[id](0), \ - submap_locations_[id](1), 0.)); - transform.setRotation(quat); - tfB_->sendTransform(tf::StampedTransform (transform, - ros::Time::now(), "/map", "/map_"+std::to_string(id))); - } - - if (feedback->event_type == \ - visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN) - { - const int id = std::stoi(feedback->marker_name,nullptr,10); - - // get yaw - tfScalar yaw, pitch, roll; - tf::Quaternion quat; - tf::quaternionMsgToTF(feedback->pose.orientation, quat); // relative - tf::Matrix3x3 mat(quat); - mat.getRPY(roll, pitch, yaw); - - // add the map_N frame there - tf::Transform transform; - transform.setOrigin(tf::Vector3(feedback->pose.position.x, \ - feedback->pose.position.y, 0.)); - transform.setRotation(quat); - tfB_->sendTransform(tf::StampedTransform (transform, - ros::Time::now(), "/map", "/map_"+std::to_string(id))); - } -} - -/*****************************************************************************/ -int main(int argc, char** argv) -/*****************************************************************************/ -{ - ros::init(argc, argv, "merge_map_tool"); - MergeMapTool mmt; - ros::spin(); - return 0; -} diff --git a/src/serialization.cpp b/src/serialization.cpp index 81f8d900d..1e1078938 100644 --- a/src/serialization.cpp +++ b/src/serialization.cpp @@ -21,37 +21,49 @@ #include #include #include +#include -#include -#include -#include -#include namespace serialization { +inline bool FileExists(const std::string& name) +{ + struct stat buffer; + return (stat (name.c_str(), &buffer) == 0); +} -void Write(const std::string& filename, karto::Mapper* mapper) +void Write(const std::string& filename, karto::Mapper* mapper, karto::Dataset* dataset) { try { - mapper->SaveToFile(filename); + mapper->SaveToFile(filename + std::string(".posegraph")); } catch (boost::archive::archive_exception e) { ROS_ERROR("Failed to write file: Exception %s", e.what()); } + std::ofstream ofs((filename + std::string(".data")).c_str()); + boost::archive::binary_oarchive oa(ofs, boost::archive::no_codecvt); + oa << BOOST_SERIALIZATION_NVP(dataset); } -void Read(const std::string& filename, karto::Mapper* mapper) +void Read(const std::string& filename, karto::Mapper* mapper, karto::Dataset*& dataset) { + if (!FileExists(filename + std::string(".posegraph"))) + { + ROS_ERROR("serialization::Read : Failed to open requested submap %s.", filename.c_str()); + } try { - mapper->LoadFromFile(filename); + mapper->LoadFromFile(filename + std::string(".posegraph")); } catch (boost::archive::archive_exception e) { ROS_ERROR("Failed to read file: Exception %s", e.what()); } + std::ifstream ifs((filename + std::string(".data")).c_str()); + boost::archive::binary_iarchive ia(ifs); + ia >> BOOST_SERIALIZATION_NVP(dataset); } } // end namespace diff --git a/src/slam_toolbox.cpp b/src/slam_toolbox.cpp index bbd2d82a4..ff5110563 100644 --- a/src/slam_toolbox.cpp +++ b/src/slam_toolbox.cpp @@ -20,7 +20,6 @@ #include #include "serialization.cpp" - /*****************************************************************************/ SlamToolbox::SlamToolbox() : transform_thread_(NULL), @@ -102,7 +101,9 @@ void SlamToolbox::SetParams(ros::NodeHandle& private_nh_) if(!private_nh_.getParam("map_frame", map_frame_)) map_frame_ = "map"; if(!private_nh_.getParam("base_frame", base_frame_)) - base_frame_ = "base_link"; + base_frame_ = "base_footprint"; + if(!private_nh_.getParam("laser_frame", laser_frame_)) + laser_frame_ = "base_laser_link"; if(!private_nh_.getParam("throttle_scans", throttle_scans_)) throttle_scans_ = 1; if(!private_nh_.getParam("publish_occupancy_map", publish_occupancy_map_)) @@ -260,6 +261,7 @@ void SlamToolbox::SetROSInterfaces(ros::NodeHandle& node) ssClear_manual_ = node.advertiseService("clear_changes", &SlamToolbox::ClearChangesCallback, this); ssSave_map_ = node.advertiseService("save_map", &SlamToolbox::SaveMapCallback, this); ssSerialize_ = node.advertiseService("serialize_map", &SlamToolbox::SerializePoseGraphCallback, this); + ssLoadMap_ = node.advertiseService("load_map", &SlamToolbox::LoadMapperCallback, this); scan_filter_sub_ = new message_filters::Subscriber(node, "/scan", 5); scan_filter_ = new tf::MessageFilter(*scan_filter_sub_, tf_, odom_frame_, 5); scan_filter_->registerCallback(boost::bind(&SlamToolbox::LaserCallback, this, _1)); @@ -417,10 +419,12 @@ karto::LaserRangeFinder* SlamToolbox::GetLaser(const \ laser->SetRangeThreshold(max_laser_range_); // Store this laser device for later - lasers_[scan->header.frame_id] = laser; - dataset_->Add(laser); + if(lasers_.find(scan->header.frame_id) == lasers_.end()) + { + lasers_[scan->header.frame_id] = laser; + dataset_->Add(laser, true); + } } - return lasers_[scan->header.frame_id]; } @@ -751,7 +755,6 @@ bool SlamToolbox::AddScan(karto::LaserRangeFinder* laser, { // Create a vector of doubles for karto std::vector readings; - if (lasers_inverted_[scan->header.frame_id]) { for(std::vector::const_reverse_iterator it = scan->ranges.rbegin(); it != scan->ranges.rend(); @@ -1066,11 +1069,74 @@ bool SlamToolbox::SerializePoseGraphCallback(slam_toolbox::SerializePoseGraph::R slam_toolbox::SerializePoseGraph::Response &resp) /*****************************************************************************/ { - const std::string filename = req.filename + std::string(".st"); - serialization::Write(filename, mapper_); + const std::string filename = req.filename; + serialization::Write(filename, mapper_, dataset_); return true; } +/*****************************************************************************/ +bool SlamToolbox::LoadMapperCallback(slam_toolbox::AddMap::Request &req, + slam_toolbox::AddMap::Response &resp) +/*****************************************************************************/ +{ + // TODO STEVE: this doesnt account for pose changes - need offset value for odometry so frames are colinear + // ie add field for initial pose of new entries in the frame of the original and a bool whether + // to continue using the existing odom in TF (when continuing the same session serialized so the odom frame hasnt changed) + // TODO STEVE2: how to remove extraneous nodes (?) + + karto::Dataset* dataset = new karto::Dataset; + karto::Mapper* mapper = new karto::Mapper; + serialization::Read(req.filename, mapper, dataset); + std::map*>> mapper_vertices = + mapper->GetGraph()->GetVertices(); + std::map*>>::iterator vertex_map_it; + for(vertex_map_it = mapper_vertices.begin(); vertex_map_it!=mapper_vertices.end(); ++vertex_map_it) + { + for(std::vector*>::iterator vertex_it= vertex_map_it->second.begin(); + vertex_it!= vertex_map_it->second.end();++vertex_it ) + { + solver_->AddNode(*vertex_it); + } + } + std::vector*> mapper_edges = mapper->GetGraph()->GetEdges(); + std::vector*>::iterator edges_it; + for( edges_it = mapper_edges.begin(); edges_it != mapper_edges.end(); ++edges_it) + { + solver_->AddConstraint(*edges_it); + } + mapper->SetScanSolver(solver_.get()); + { + boost::mutex::scoped_lock lock(mapper_mutex_); + if (mapper_) + { + delete mapper_; + mapper_ = NULL; + } + if (dataset_) + { + delete dataset_; + dataset_ = NULL; + } + + karto::LaserRangeFinder* laser = dynamic_cast(dataset->GetObjects()[0]); + mapper_ = mapper; + dataset_ = dataset; + karto::Sensor* pSensor = dynamic_cast(laser); + if (pSensor) + { + karto::SensorManager::GetInstance()->RegisterSensor(pSensor); + lasers_[laser_frame_] = laser; + bool is_inverted; + if(!nh_.getParam("inverted_laser", is_inverted)) + { + is_inverted = false; + } + lasers_inverted_[laser_frame_] = is_inverted; + } + } + UpdateMap(); +} + /*****************************************************************************/ int main(int argc, char** argv) /*****************************************************************************/ diff --git a/srvs/AddMap.srv b/srvs/AddMap.srv new file mode 100644 index 000000000..811117977 --- /dev/null +++ b/srvs/AddMap.srv @@ -0,0 +1,2 @@ +string filename +--- \ No newline at end of file