From 490e7a814520f93ba3ea85ee9bbce7f433952808 Mon Sep 17 00:00:00 2001 From: suryakanth Date: Fri, 11 Nov 2022 15:53:26 +0530 Subject: [PATCH 01/13] slam_toolbox::graph constrain visualizations --- .../slam_toolbox/loop_closure_assistant.hpp | 6 +- slam_toolbox/src/loop_closure_assistant.cpp | 556 ++++++--- slam_toolbox/src/slam_toolbox_common.cpp | 1093 +++++++++-------- 3 files changed, 907 insertions(+), 748 deletions(-) diff --git a/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp b/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp index 6855514f0..d15c5cff1 100644 --- a/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp +++ b/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp @@ -47,13 +47,16 @@ class LoopClosureAssistant void clearMovedNodes(); void processInteractiveFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); void publishGraph(); - + void setMapper(karto::Mapper * mapper); private: bool manualLoopClosureCallback(slam_toolbox_msgs::LoopClosure::Request& req, slam_toolbox_msgs::LoopClosure::Response& resp); bool clearChangesCallback(slam_toolbox_msgs::Clear::Request& req, slam_toolbox_msgs::Clear::Response& resp); bool interactiveModeCallback(slam_toolbox_msgs::ToggleInteractive::Request &req, slam_toolbox_msgs::ToggleInteractive::Response &resp); void moveNode(const int& id, const Eigen::Vector3d& pose); void addMovedNodes(const int& id, Eigen::Vector3d vec); + std_msgs::ColorRGBA getColor(float r, float g, float b); + visualization_msgs::Marker getMarker( std::string name,int reserve); + std::unique_ptr tfB_; laser_utils::ScanHolder* scan_holder_; @@ -70,6 +73,7 @@ class LoopClosureAssistant std::string map_frame_; PausedState& state_; ProcessType& processor_type_; + bool isInter = false; }; } // end namespace diff --git a/slam_toolbox/src/loop_closure_assistant.cpp b/slam_toolbox/src/loop_closure_assistant.cpp index b10ea0905..9f88db97a 100644 --- a/slam_toolbox/src/loop_closure_assistant.cpp +++ b/slam_toolbox/src/loop_closure_assistant.cpp @@ -21,272 +21,426 @@ namespace loop_closure_assistant { -/*****************************************************************************/ -LoopClosureAssistant::LoopClosureAssistant( + /*****************************************************************************/ + LoopClosureAssistant::LoopClosureAssistant( ros::NodeHandle& node, karto::Mapper* mapper, laser_utils::ScanHolder* scan_holder, PausedState& state, ProcessType & processor_type) -: mapper_(mapper), scan_holder_(scan_holder), - interactive_mode_(false), nh_(node), state_(state), - processor_type_(processor_type) -/*****************************************************************************/ -{ - node.setParam("paused_processing", false); - tfB_ = std::make_unique(); - ssClear_manual_ = node.advertiseService("clear_changes", - &LoopClosureAssistant::clearChangesCallback, this); - ssLoopClosure_ = node.advertiseService("manual_loop_closure", - &LoopClosureAssistant::manualLoopClosureCallback, this); - scan_publisher_ = node.advertise( + : mapper_(mapper), scan_holder_(scan_holder), + interactive_mode_(false), nh_(node), state_(state), + processor_type_(processor_type) + /*****************************************************************************/ + { + node.setParam("paused_processing", false); + tfB_ = std::make_unique(); + ssClear_manual_ = node.advertiseService("clear_changes", + &LoopClosureAssistant::clearChangesCallback, this); + ssLoopClosure_ = node.advertiseService("manual_loop_closure", + &LoopClosureAssistant::manualLoopClosureCallback, this); + scan_publisher_ = node.advertise( "karto_scan_visualization",10); - solver_ = mapper_->getScanSolver(); - interactive_server_ = - std::make_unique( + solver_ = mapper_->getScanSolver(); + interactive_server_ = + std::make_unique( "slam_toolbox","",true); - ssInteractive_ = node.advertiseService("toggle_interactive_mode", + ssInteractive_ = node.advertiseService("toggle_interactive_mode", &LoopClosureAssistant::interactiveModeCallback,this); - node.setParam("interactive_mode", interactive_mode_); - marker_publisher_ = node.advertise( + node.setParam("interactive_mode", interactive_mode_); + marker_publisher_ = node.advertise( "karto_graph_visualization",1); - node.param("map_frame", map_frame_, std::string("map")); - node.param("enable_interactive_mode", enable_interactive_mode_, false); -} + node.param("map_frame", map_frame_, std::string("map")); + node.param("enable_interactive_mode", enable_interactive_mode_, false); + } + /*****************************************************************************/ + void LoopClosureAssistant::setMapper(karto::Mapper *mapper) + /*****************************************************************************/ + { + mapper_ = mapper; + } -/*****************************************************************************/ + /*****************************************************************************/ void LoopClosureAssistant::processInteractiveFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) -/*****************************************************************************/ -{ - if (processor_type_ != PROCESS) + /*****************************************************************************/ { - ROS_ERROR_THROTTLE(5., - "Interactive mode is invalid outside processing mode."); - return; - } + if (processor_type_ != PROCESS) + { + ROS_ERROR_THROTTLE(5., + "Interactive mode is invalid outside processing mode."); + return; + } - const int id = std::stoi(feedback->marker_name, nullptr, 10) - 1; + const int id = std::stoi(feedback->marker_name, nullptr, 10) - 1; - // was depressed, something moved, and now released - if (feedback->event_type == - visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP && - feedback->mouse_point_valid) - { - addMovedNodes(id, Eigen::Vector3d(feedback->mouse_point.x, - feedback->mouse_point.y, tf2::getYaw(feedback->pose.orientation))); - } + // was depressed, something moved, and now released + if (feedback->event_type == + visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP && + feedback->mouse_point_valid) + { + addMovedNodes(id, Eigen::Vector3d(feedback->mouse_point.x, + feedback->mouse_point.y, tf2::getYaw(feedback->pose.orientation))); + } - // is currently depressed, being moved before release - if (feedback->event_type == - visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE) - { - // get scan - sensor_msgs::LaserScan scan = scan_holder_->getCorrectedScan(id); + // is currently depressed, being moved before release + if (feedback->event_type == + visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE) + { + // get scan + sensor_msgs::LaserScan scan = scan_holder_->getCorrectedScan(id); - // get correct orientation + // get correct orientation tf2::Quaternion quat(0.,0.,0.,1.0), msg_quat(0.,0.,0.,1.0); - double node_yaw, first_node_yaw; - solver_->GetNodeOrientation(id, node_yaw); - solver_->GetNodeOrientation(0, first_node_yaw); + double node_yaw, first_node_yaw; + solver_->GetNodeOrientation(id, node_yaw); + solver_->GetNodeOrientation(0, first_node_yaw); tf2::Quaternion q1(0.,0.,0.,1.0); - q1.setEuler(0., 0., node_yaw - 3.14159); + q1.setEuler(0., 0., node_yaw - 3.14159); tf2::Quaternion q2(0.,0.,0.,1.0); - q2.setEuler(0., 0., 3.14159); - quat *= q1; - quat *= q2; - - // interactive move - tf2::convert(feedback->pose.orientation, msg_quat); - quat *= msg_quat; - quat.normalize(); - - // create correct transform - tf2::Transform transform; - transform.setOrigin(tf2::Vector3(feedback->pose.position.x, - feedback->pose.position.y, 0.)); - transform.setRotation(quat); - - // publish the scan visualization with transform - geometry_msgs::TransformStamped msg; - tf2::convert(transform, msg.transform); - msg.child_frame_id = "karto_scan_visualization"; - msg.header.frame_id = feedback->header.frame_id; - msg.header.stamp = ros::Time::now(); - tfB_->sendTransform(msg); - - scan.header.frame_id = "karto_scan_visualization"; - scan.header.stamp = ros::Time::now(); - scan_publisher_.publish(scan); + q2.setEuler(0., 0., 3.14159); + quat *= q1; + quat *= q2; + + // interactive move + tf2::convert(feedback->pose.orientation, msg_quat); + quat *= msg_quat; + quat.normalize(); + + // create correct transform + tf2::Transform transform; + transform.setOrigin(tf2::Vector3(feedback->pose.position.x, + feedback->pose.position.y, 0.)); + transform.setRotation(quat); + + // publish the scan visualization with transform + geometry_msgs::TransformStamped msg; + tf2::convert(transform, msg.transform); + msg.child_frame_id = "karto_scan_visualization"; + msg.header.frame_id = feedback->header.frame_id; + msg.header.stamp = ros::Time::now(); + tfB_->sendTransform(msg); + + scan.header.frame_id = "karto_scan_visualization"; + scan.header.stamp = ros::Time::now(); + scan_publisher_.publish(scan); + } } -} -/*****************************************************************************/ -void LoopClosureAssistant::publishGraph() -/*****************************************************************************/ -{ - interactive_server_->clear(); - std::unordered_map* graph = solver_->getGraph(); - - if (graph->size() == 0) + std_msgs::ColorRGBA LoopClosureAssistant::getColor(float r, float g, float b) { - return; + std_msgs::ColorRGBA color; + color.r = r; + color.g = g; + color.b = b; + return color; } - ROS_DEBUG("Graph size: %i",(int)graph->size()); - bool interactive_mode = false; + visualization_msgs::Marker LoopClosureAssistant::getMarker(std::string name, int reserve) { - boost::mutex::scoped_lock lock(interactive_mutex_); - interactive_mode = interactive_mode_; + visualization_msgs::Marker edges_marker; + edges_marker.header.frame_id = map_frame_; + edges_marker.header.stamp = ros::Time::now(); + edges_marker.id = 0; + edges_marker.ns = name; + edges_marker.action = visualization_msgs::Marker::ADD; + edges_marker.type = visualization_msgs::Marker::LINE_LIST; + edges_marker.pose.orientation.w = 1; + edges_marker.scale.x = 0.01; + edges_marker.color.r = 0.0; + edges_marker.color.g = 0.0; + edges_marker.color.b = 0.0; + edges_marker.color.a = 1; + edges_marker.lifetime = ros::Duration(0.0); + edges_marker.points.reserve(reserve); + return edges_marker; } - visualization_msgs::MarkerArray marray; - visualization_msgs::Marker m = vis_utils::toMarker(map_frame_, - "slam_toolbox", 0.1); - - for (ConstGraphIterator it = graph->begin(); it != graph->end(); ++it) + /*****************************************************************************/ + void LoopClosureAssistant::publishGraph() + /*****************************************************************************/ { - m.id = it->first + 1; - m.pose.position.x = it->second(0); - m.pose.position.y = it->second(1); + interactive_server_->clear(); + std::unordered_map* graph = solver_->getGraph(); - if (interactive_mode && enable_interactive_mode_) + if (graph->size() == 0) { - visualization_msgs::InteractiveMarker int_marker = - vis_utils::toInteractiveMarker(m, 0.3); - interactive_server_->insert(int_marker, + return; + } + + ROS_DEBUG("Graph size: %i",(int)graph->size()); + bool interactive_mode = false; + { + boost::mutex::scoped_lock lock(interactive_mutex_); + interactive_mode = interactive_mode_; + } + + if (!mapper_->GetGraph()) + ROS_ERROR_STREAM("Invalid pointer"); + + const auto &vertices = mapper_->GetGraph()->GetVertices(); + + const auto &edges = mapper_->GetGraph()->GetEdges(); + + const auto &localization_vertices = mapper_->GetLocalizationVertices(); + + int first_localization_id = std::numeric_limits::max(); + if (!localization_vertices.empty()) + { + first_localization_id = localization_vertices.front().vertex->GetObject()->GetUniqueId(); + } + + visualization_msgs::MarkerArray marray; + + // clear existing markers to account for any removed nodes + visualization_msgs::Marker clear; + clear.header.stamp = ros::Time::now(); + clear.action = visualization_msgs::Marker::DELETEALL; + marray.markers.push_back(clear); + + visualization_msgs::Marker slam_node = vis_utils::toMarker(map_frame_, "slam_nodes", 0.05); + visualization_msgs::Marker loc_node = vis_utils::toMarker(map_frame_, "loc_node", 0.05); + + // add map nodes + for (const auto &sensor_name : vertices) + { + for (const auto &vertex : sensor_name.second) + { + + visualization_msgs::Marker m; + if (vertex.first < first_localization_id) + m = slam_node; + else + { + m = loc_node; + m.color.r = 0.0; + m.color.b = 0.0; + } + + const auto &pose = vertex.second->GetObject()->GetCorrectedPose(); + m.id = vertex.first; + m.pose.position.x = pose.GetX(); + m.pose.position.y = pose.GetY(); + + if (interactive_mode && enable_interactive_mode_) + { + visualization_msgs::InteractiveMarker int_marker = + vis_utils::toInteractiveMarker(m, 0.3); + interactive_server_->insert(int_marker, boost::bind( - &LoopClosureAssistant::processInteractiveFeedback, + &LoopClosureAssistant::processInteractiveFeedback, this, _1)); + } + else + { + marray.markers.push_back(m); + } + } } - else + + // add line markers for graph edges + visualization_msgs::Marker edges_marker = getMarker("intra_slam_edges", edges.size() * 2); + visualization_msgs::Marker edges_marker_inter = getMarker("inter_slam_edges", edges.size() * 2); + visualization_msgs::Marker loc_edges_marker = getMarker("intra_loc_edges", localization_vertices.size() * 3); + visualization_msgs::Marker inter_loc_edges_marker = getMarker("inter_loc_edges", localization_vertices.size() * 3); + + geometry_msgs::Point prevSourcePoint, prevTargetPoint; + for (const auto &edge : edges) { - marray.markers.push_back(m); + isInter = false; + + std_msgs::ColorRGBA color; + + color.a = 0.3; + + int source_id = edge->GetSource()->GetObject()->GetUniqueId(); + const auto &pose0 = edge->GetSource()->GetObject()->GetCorrectedPose(); + geometry_msgs::Point sourcePoint; + sourcePoint.x = pose0.GetX(); + sourcePoint.y = pose0.GetY(); + // if (prevTargetPoint == sourcePoint) + + int target_id = edge->GetTarget()->GetObject()->GetUniqueId(); + const auto &pose1 = edge->GetTarget()->GetObject()->GetCorrectedPose(); + geometry_msgs::Point targetPoint; + targetPoint.x = pose1.GetX(); + targetPoint.y = pose1.GetY(); + if (prevTargetPoint == targetPoint) + { + isInter = true; + color = (source_id >= first_localization_id) ? getColor(1, 0, 1) : getColor(0, 1, 1); + } + else + { + color = getColor(1, 1, 0); // yello + } + + prevSourcePoint = sourcePoint; + prevTargetPoint = targetPoint; + + if (source_id >= first_localization_id || target_id >= first_localization_id) + { + if (isInter) + { + inter_loc_edges_marker.colors.push_back(color); + inter_loc_edges_marker.colors.push_back(color); + inter_loc_edges_marker.points.push_back(sourcePoint); + inter_loc_edges_marker.points.push_back(targetPoint); + } + else + { + loc_edges_marker.colors.push_back(color); + loc_edges_marker.colors.push_back(color); + loc_edges_marker.points.push_back(sourcePoint); + loc_edges_marker.points.push_back(targetPoint); + } + } + else + { + color.g = 0; + if (isInter) + { + edges_marker_inter.colors.push_back(color); + edges_marker_inter.colors.push_back(color); + edges_marker_inter.points.push_back(sourcePoint); + edges_marker_inter.points.push_back(targetPoint); + } + else + { + edges_marker.colors.push_back(color); + edges_marker.colors.push_back(color); + edges_marker.points.push_back(sourcePoint); + edges_marker.points.push_back(targetPoint); + } + } } - } - - // if disabled, clears out old markers - interactive_server_->applyChanges(); - marker_publisher_.publish(marray); + marray.markers.push_back(edges_marker_inter); + marray.markers.push_back(edges_marker); + marray.markers.push_back(inter_loc_edges_marker); + marray.markers.push_back(loc_edges_marker); + + // if disabled, clears out old markers + interactive_server_->applyChanges(); + marker_publisher_.publish(marray); return; -} + } -/*****************************************************************************/ -bool LoopClosureAssistant::manualLoopClosureCallback( + /*****************************************************************************/ + bool LoopClosureAssistant::manualLoopClosureCallback( slam_toolbox_msgs::LoopClosure::Request& req, slam_toolbox_msgs::LoopClosure::Response& resp) -/*****************************************************************************/ -{ - if(!enable_interactive_mode_) - { - ROS_WARN("Called manual loop closure" - " with interactive mode disabled. Ignoring."); - return false; - } - + /*****************************************************************************/ { - boost::mutex::scoped_lock lock(moved_nodes_mutex_); - - if (moved_nodes_.size() == 0) + if(!enable_interactive_mode_) { - ROS_WARN("No moved nodes to attempt manual loop closure."); - return true; + ROS_WARN("Called manual loop closure" + " with interactive mode disabled. Ignoring."); + return false; } - ROS_INFO("LoopClosureAssistant: Attempting to manual " - "loop close with %i moved nodes.", (int)moved_nodes_.size()); - // for each in node map - std::map::const_iterator it = moved_nodes_.begin(); - for (it; it != moved_nodes_.end(); ++it) { - moveNode(it->first, + boost::mutex::scoped_lock lock(moved_nodes_mutex_); + + if (moved_nodes_.size() == 0) + { + ROS_WARN("No moved nodes to attempt manual loop closure."); + return true; + } + + ROS_INFO("LoopClosureAssistant: Attempting to manual " + "loop close with %i moved nodes.", (int)moved_nodes_.size()); + // for each in node map + std::map::const_iterator it = moved_nodes_.begin(); + for (it; it != moved_nodes_.end(); ++it) + { + moveNode(it->first, Eigen::Vector3d(it->second(0),it->second(1), it->second(2))); + } } - } - // optimize - mapper_->CorrectPoses(); + // optimize + mapper_->CorrectPoses(); - // update visualization and clear out nodes completed - publishGraph(); - clearMovedNodes(); - return true; -} + // update visualization and clear out nodes completed + publishGraph(); + clearMovedNodes(); + return true; + } -/*****************************************************************************/ -bool LoopClosureAssistant::interactiveModeCallback( + /*****************************************************************************/ + bool LoopClosureAssistant::interactiveModeCallback( slam_toolbox_msgs::ToggleInteractive::Request &req, - slam_toolbox_msgs::ToggleInteractive::Response &resp) -/*****************************************************************************/ -{ - if(!enable_interactive_mode_) + slam_toolbox_msgs::ToggleInteractive::Response &resp) + /*****************************************************************************/ { - ROS_WARN("Called toggle interactive mode with " - "interactive mode disabled. Ignoring."); - return false; - } + if(!enable_interactive_mode_) + { + ROS_WARN("Called toggle interactive mode with " + "interactive mode disabled. Ignoring."); + return false; + } - bool interactive_mode; - { - boost::mutex::scoped_lock lock_i(interactive_mutex_); - interactive_mode_ = !interactive_mode_; - interactive_mode = interactive_mode_; - nh_.setParam("interactive_mode", interactive_mode_); - } + bool interactive_mode; + { + boost::mutex::scoped_lock lock_i(interactive_mutex_); + interactive_mode_ = !interactive_mode_; + interactive_mode = interactive_mode_; + nh_.setParam("interactive_mode", interactive_mode_); + } - ROS_INFO("SlamToolbox: Toggling %s interactive mode.", - interactive_mode ? "on" : "off"); - publishGraph(); - clearMovedNodes(); + ROS_INFO("SlamToolbox: Toggling %s interactive mode.", + interactive_mode ? "on" : "off"); + publishGraph(); + clearMovedNodes(); - // set state so we don't overwrite changes in rviz while loop closing - state_.set(PROCESSING, interactive_mode); - state_.set(VISUALIZING_GRAPH, interactive_mode); - nh_.setParam("paused_processing", interactive_mode); - return true; -} + // set state so we don't overwrite changes in rviz while loop closing + state_.set(PROCESSING, interactive_mode); + state_.set(VISUALIZING_GRAPH, interactive_mode); + nh_.setParam("paused_processing", interactive_mode); + return true; + } -/*****************************************************************************/ -void LoopClosureAssistant::moveNode( + /*****************************************************************************/ + void LoopClosureAssistant::moveNode( const int& id, const Eigen::Vector3d& pose) -/*****************************************************************************/ -{ - solver_->ModifyNode(id, pose); -} + /*****************************************************************************/ + { + solver_->ModifyNode(id, pose); + } -/*****************************************************************************/ -bool LoopClosureAssistant::clearChangesCallback( + /*****************************************************************************/ + bool LoopClosureAssistant::clearChangesCallback( slam_toolbox_msgs::Clear::Request& req, slam_toolbox_msgs::Clear::Response& resp) -/*****************************************************************************/ -{ - if(!enable_interactive_mode_) + /*****************************************************************************/ { - ROS_WARN("Called Clear changes with interactive mode disabled. Ignoring."); - return false; - } + if(!enable_interactive_mode_) + { + ROS_WARN("Called Clear changes with interactive mode disabled. Ignoring."); + return false; + } - ROS_INFO("LoopClosureAssistant: Clearing manual loop closure nodes."); - publishGraph(); - clearMovedNodes(); - return true; -} + ROS_INFO("LoopClosureAssistant: Clearing manual loop closure nodes."); + publishGraph(); + clearMovedNodes(); + return true; + } -/*****************************************************************************/ + /*****************************************************************************/ void LoopClosureAssistant::clearMovedNodes() -/*****************************************************************************/ -{ - boost::mutex::scoped_lock lock(moved_nodes_mutex_); - moved_nodes_.clear(); -} + /*****************************************************************************/ + { + boost::mutex::scoped_lock lock(moved_nodes_mutex_); + moved_nodes_.clear(); + } -/*****************************************************************************/ + /*****************************************************************************/ void LoopClosureAssistant::addMovedNodes(const int& id, Eigen::Vector3d vec) -/*****************************************************************************/ -{ - ROS_INFO("LoopClosureAssistant: Node %i new manual loop closure " + /*****************************************************************************/ + { + ROS_INFO("LoopClosureAssistant: Node %i new manual loop closure " "pose has been recorded.",id); - boost::mutex::scoped_lock lock(moved_nodes_mutex_); - moved_nodes_[id] = vec; -} + boost::mutex::scoped_lock lock(moved_nodes_mutex_); + moved_nodes_[id] = vec; + } } // end namespace \ No newline at end of file diff --git a/slam_toolbox/src/slam_toolbox_common.cpp b/slam_toolbox/src/slam_toolbox_common.cpp index a778fbb26..0e217ac41 100644 --- a/slam_toolbox/src/slam_toolbox_common.cpp +++ b/slam_toolbox/src/slam_toolbox_common.cpp @@ -24,741 +24,742 @@ namespace slam_toolbox { -/*****************************************************************************/ -SlamToolbox::SlamToolbox(ros::NodeHandle& nh) -: solver_loader_("slam_toolbox", "karto::ScanSolver"), - processor_type_(PROCESS), - first_measurement_(true), - nh_(nh), - process_near_pose_(nullptr) -/*****************************************************************************/ -{ - smapper_ = std::make_unique(); - dataset_ = std::make_unique(); - - setParams(nh_); - setROSInterfaces(nh_); - setSolver(nh_); - - laser_assistant_ = std::make_unique( - nh_, tf_.get(), base_frame_); - pose_helper_ = std::make_unique( - tf_.get(), base_frame_, odom_frame_); - scan_holder_ = std::make_unique(lasers_); - map_saver_ = std::make_unique(nh_, map_name_); - closure_assistant_ = - std::make_unique( - nh_, smapper_->getMapper(), scan_holder_.get(), state_, processor_type_); - - reprocessing_transform_.setIdentity(); - - double transform_publish_period; - nh_.param("transform_publish_period", transform_publish_period, 0.05); - threads_.push_back(std::make_unique( - boost::bind(&SlamToolbox::publishTransformLoop, - this, transform_publish_period))); - threads_.push_back(std::make_unique( - boost::bind(&SlamToolbox::publishVisualizations, this))); -} - -/*****************************************************************************/ -SlamToolbox::~SlamToolbox() -/*****************************************************************************/ -{ - for (int i=0; i != threads_.size(); i++) + /*****************************************************************************/ + SlamToolbox::SlamToolbox(ros::NodeHandle& nh) + : solver_loader_("slam_toolbox", "karto::ScanSolver"), + processor_type_(PROCESS), + first_measurement_(true), + nh_(nh), + process_near_pose_(nullptr) + /*****************************************************************************/ + { + smapper_ = std::make_unique(); + dataset_ = std::make_unique(); + + setParams(nh_); + setROSInterfaces(nh_); + setSolver(nh_); + + laser_assistant_ = std::make_unique( + nh_, tf_.get(), base_frame_); + pose_helper_ = std::make_unique( + tf_.get(), base_frame_, odom_frame_); + scan_holder_ = std::make_unique(lasers_); + map_saver_ = std::make_unique(nh_, map_name_); + closure_assistant_ = + std::make_unique( + nh_, smapper_->getMapper(), scan_holder_.get(), state_, processor_type_); + + reprocessing_transform_.setIdentity(); + + double transform_publish_period; + nh_.param("transform_publish_period", transform_publish_period, 0.05); + threads_.push_back(std::make_unique( + boost::bind(&SlamToolbox::publishTransformLoop, + this, transform_publish_period))); + threads_.push_back(std::make_unique( + boost::bind(&SlamToolbox::publishVisualizations, this))); + } + + /*****************************************************************************/ + SlamToolbox::~SlamToolbox() + /*****************************************************************************/ { - threads_[i]->join(); - } + for (int i=0; i != threads_.size(); i++) + { + threads_[i]->join(); + } - smapper_.reset(); - dataset_.reset(); - closure_assistant_.reset(); - map_saver_.reset(); - pose_helper_.reset(); - laser_assistant_.reset(); - scan_holder_.reset(); -} + smapper_.reset(); + dataset_.reset(); + closure_assistant_.reset(); + map_saver_.reset(); + pose_helper_.reset(); + laser_assistant_.reset(); + scan_holder_.reset(); + } -/*****************************************************************************/ + /*****************************************************************************/ void SlamToolbox::setSolver(ros::NodeHandle& private_nh_) -/*****************************************************************************/ -{ - // Set solver to be used in loop closure - std::string solver_plugin; - if(!private_nh_.getParam("solver_plugin", solver_plugin)) - { - ROS_WARN("unable to find requested solver plugin, defaulting to SPA"); - solver_plugin = "solver_plugins::CeresSolver"; - } - try + /*****************************************************************************/ { - solver_ = solver_loader_.createInstance(solver_plugin); - ROS_INFO("Using plugin %s", solver_plugin.c_str()); - } + // Set solver to be used in loop closure + std::string solver_plugin; + if(!private_nh_.getParam("solver_plugin", solver_plugin)) + { + ROS_WARN("unable to find requested solver plugin, defaulting to SPA"); + solver_plugin = "solver_plugins::CeresSolver"; + } + try + { + solver_ = solver_loader_.createInstance(solver_plugin); + ROS_INFO("Using plugin %s", solver_plugin.c_str()); + } catch (const pluginlib::PluginlibException& ex) - { - ROS_FATAL("Failed to create %s, is it registered and built? Exception: %s.", - solver_plugin.c_str(), ex.what()); - exit(1); + { + ROS_FATAL("Failed to create %s, is it registered and built? Exception: %s.", + solver_plugin.c_str(), ex.what()); + exit(1); + } + smapper_->getMapper()->SetScanSolver(solver_.get()); } - smapper_->getMapper()->SetScanSolver(solver_.get()); -} -/*****************************************************************************/ + /*****************************************************************************/ void SlamToolbox::setParams(ros::NodeHandle& private_nh) -/*****************************************************************************/ -{ - map_to_odom_.setIdentity(); - private_nh.param("odom_frame", odom_frame_, std::string("odom")); - private_nh.param("map_frame", map_frame_, std::string("map")); - private_nh.param("base_frame", base_frame_, std::string("base_footprint")); - private_nh.param("resolution", resolution_, 0.05); - private_nh.param("map_name", map_name_, std::string("/map")); - private_nh.param("scan_topic", scan_topic_, std::string("/scan")); - private_nh.param("throttle_scans", throttle_scans_, 1); - private_nh.param("enable_interactive_mode", enable_interactive_mode_, false); - - double tmp_val; - private_nh.param("transform_timeout", tmp_val, 0.2); - transform_timeout_ = ros::Duration(tmp_val); - private_nh.param("tf_buffer_duration", tmp_val, 30.); - tf_buffer_dur_ = ros::Duration(tmp_val); - private_nh.param("minimum_time_interval", tmp_val, 0.5); - minimum_time_interval_ = ros::Duration(tmp_val); - - bool debug = false; - if (private_nh.getParam("debug_logging", debug) && debug) - { - if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, - ros::console::levels::Debug)) + /*****************************************************************************/ + { + map_to_odom_.setIdentity(); + private_nh.param("odom_frame", odom_frame_, std::string("odom")); + private_nh.param("map_frame", map_frame_, std::string("map")); + private_nh.param("base_frame", base_frame_, std::string("base_footprint")); + private_nh.param("resolution", resolution_, 0.05); + private_nh.param("map_name", map_name_, std::string("/map")); + private_nh.param("scan_topic", scan_topic_, std::string("/scan")); + private_nh.param("throttle_scans", throttle_scans_, 1); + private_nh.param("enable_interactive_mode", enable_interactive_mode_, false); + + double tmp_val; + private_nh.param("transform_timeout", tmp_val, 0.2); + transform_timeout_ = ros::Duration(tmp_val); + private_nh.param("tf_buffer_duration", tmp_val, 30.); + tf_buffer_dur_ = ros::Duration(tmp_val); + private_nh.param("minimum_time_interval", tmp_val, 0.5); + minimum_time_interval_ = ros::Duration(tmp_val); + + bool debug = false; + if (private_nh.getParam("debug_logging", debug) && debug) { - ros::console::notifyLoggerLevelsChanged(); + if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, + ros::console::levels::Debug)) + { + ros::console::notifyLoggerLevelsChanged(); + } } - } - smapper_->configure(private_nh); - private_nh.setParam("paused_new_measurements", false); -} + smapper_->configure(private_nh); + private_nh.setParam("paused_new_measurements", false); + } -/*****************************************************************************/ + /*****************************************************************************/ void SlamToolbox::setROSInterfaces(ros::NodeHandle& node) -/*****************************************************************************/ -{ - tf_ = std::make_unique(ros::Duration(tf_buffer_dur_)); - tfL_ = std::make_unique(*tf_); - tfB_ = std::make_unique(); - sst_ = node.advertise(map_name_, 1, true); - sstm_ = node.advertise(map_name_ + "_metadata", 1, true); - ssMap_ = node.advertiseService("dynamic_map", &SlamToolbox::mapCallback, this); - ssPauseMeasurements_ = node.advertiseService("pause_new_measurements", &SlamToolbox::pauseNewMeasurementsCallback, this); - ssSerialize_ = node.advertiseService("serialize_map", &SlamToolbox::serializePoseGraphCallback, this); - ssDesserialize_ = node.advertiseService("deserialize_map", &SlamToolbox::deserializePoseGraphCallback, this); + /*****************************************************************************/ + { + tf_ = std::make_unique(ros::Duration(tf_buffer_dur_)); + tfL_ = std::make_unique(*tf_); + tfB_ = std::make_unique(); + sst_ = node.advertise(map_name_, 1, true); + sstm_ = node.advertise(map_name_ + "_metadata", 1, true); + ssMap_ = node.advertiseService("dynamic_map", &SlamToolbox::mapCallback, this); + ssPauseMeasurements_ = node.advertiseService("pause_new_measurements", &SlamToolbox::pauseNewMeasurementsCallback, this); + ssSerialize_ = node.advertiseService("serialize_map", &SlamToolbox::serializePoseGraphCallback, this); + ssDesserialize_ = node.advertiseService("deserialize_map", &SlamToolbox::deserializePoseGraphCallback, this); scan_filter_sub_ = std::make_unique >(node, scan_topic_, 5); scan_filter_ = std::make_unique >(*scan_filter_sub_, *tf_, odom_frame_, 5, node); - scan_filter_->registerCallback(boost::bind(&SlamToolbox::laserCallback, this, _1)); -} + scan_filter_->registerCallback(boost::bind(&SlamToolbox::laserCallback, this, _1)); + } -/*****************************************************************************/ + /*****************************************************************************/ void SlamToolbox::publishTransformLoop(const double& transform_publish_period) -/*****************************************************************************/ -{ - if(transform_publish_period == 0) + /*****************************************************************************/ { - return; - } + if(transform_publish_period == 0) + { + return; + } - ros::Rate r(1.0 / transform_publish_period); + ros::Rate r(1.0 / transform_publish_period); while(ros::ok()) - { { - boost::mutex::scoped_lock lock(map_to_odom_mutex_); - geometry_msgs::TransformStamped msg; - tf2::convert(map_to_odom_, msg.transform); - msg.child_frame_id = odom_frame_; - msg.header.frame_id = map_frame_; - msg.header.stamp = ros::Time::now() + transform_timeout_; - tfB_->sendTransform(msg); + { + boost::mutex::scoped_lock lock(map_to_odom_mutex_); + geometry_msgs::TransformStamped msg; + tf2::convert(map_to_odom_, msg.transform); + msg.child_frame_id = odom_frame_; + msg.header.frame_id = map_frame_; + msg.header.stamp = ros::Time::now() + transform_timeout_; + tfB_->sendTransform(msg); + } + r.sleep(); } - r.sleep(); } -} -/*****************************************************************************/ -void SlamToolbox::publishVisualizations() -/*****************************************************************************/ -{ + /*****************************************************************************/ + void SlamToolbox::publishVisualizations() + /*****************************************************************************/ + { nav_msgs::OccupancyGrid& og = map_.map; - og.info.resolution = resolution_; - og.info.origin.position.x = 0.0; - og.info.origin.position.y = 0.0; - og.info.origin.position.z = 0.0; - og.info.origin.orientation.x = 0.0; - og.info.origin.orientation.y = 0.0; - og.info.origin.orientation.z = 0.0; - og.info.origin.orientation.w = 1.0; - og.header.frame_id = map_frame_; - - double map_update_interval; + og.info.resolution = resolution_; + og.info.origin.position.x = 0.0; + og.info.origin.position.y = 0.0; + og.info.origin.position.z = 0.0; + og.info.origin.orientation.x = 0.0; + og.info.origin.orientation.y = 0.0; + og.info.origin.orientation.z = 0.0; + og.info.origin.orientation.w = 1.0; + og.header.frame_id = map_frame_; + + double map_update_interval; if(!nh_.getParam("map_update_interval", map_update_interval)) - map_update_interval = 10.0; + map_update_interval = 10.0; ros::Rate r(1.0 / map_update_interval); while(ros::ok()) - { - updateMap(); + { + updateMap(); if(!isPaused(VISUALIZING_GRAPH)) { closure_assistant_->publishGraph(); } - r.sleep(); + r.sleep(); + } } -} -/*****************************************************************************/ + /*****************************************************************************/ void SlamToolbox::loadPoseGraphByParams(ros::NodeHandle& nh) -/*****************************************************************************/ -{ - std::string filename; - geometry_msgs::Pose2D pose; - bool dock = false; - if (shouldStartWithPoseGraph(filename, pose, dock)) + /*****************************************************************************/ { - slam_toolbox_msgs::DeserializePoseGraph::Request req; - slam_toolbox_msgs::DeserializePoseGraph::Response resp; - req.initial_pose = pose; - req.filename = filename; - if (dock) - { - req.match_type = - slam_toolbox_msgs::DeserializePoseGraph::Request::START_AT_FIRST_NODE; - } - else + std::string filename; + geometry_msgs::Pose2D pose; + bool dock = false; + if (shouldStartWithPoseGraph(filename, pose, dock)) { - req.match_type = - slam_toolbox_msgs::DeserializePoseGraph::Request::START_AT_GIVEN_POSE; + slam_toolbox_msgs::DeserializePoseGraph::Request req; + slam_toolbox_msgs::DeserializePoseGraph::Response resp; + req.initial_pose = pose; + req.filename = filename; + if (dock) + { + req.match_type = + slam_toolbox_msgs::DeserializePoseGraph::Request::START_AT_FIRST_NODE; + } + else + { + req.match_type = + slam_toolbox_msgs::DeserializePoseGraph::Request::START_AT_GIVEN_POSE; + } + deserializePoseGraphCallback(req, resp); } - deserializePoseGraphCallback(req, resp); } -} -/*****************************************************************************/ + /*****************************************************************************/ bool SlamToolbox::shouldStartWithPoseGraph(std::string& filename, geometry_msgs::Pose2D& pose, bool& start_at_dock) -/*****************************************************************************/ + /*****************************************************************************/ { - // if given a map to load at run time, do it. - if (nh_.getParam("map_file_name", filename)) - { - std::vector read_pose; - if (nh_.getParam("map_start_pose", read_pose)) + // if given a map to load at run time, do it. + if (nh_.getParam("map_file_name", filename)) { - start_at_dock = false; - if (read_pose.size() != 3) + std::vector read_pose; + if (nh_.getParam("map_start_pose", read_pose)) { - ROS_ERROR("LocalizationSlamToolbox: Incorrect number of " - "arguments for map starting pose. Must be in format: " - "[x, y, theta]. Starting at the origin"); - pose.x = 0.; - pose.y = 0.; - pose.theta = 0.; + start_at_dock = false; + if (read_pose.size() != 3) + { + ROS_ERROR("LocalizationSlamToolbox: Incorrect number of " + "arguments for map starting pose. Must be in format: " + "[x, y, theta]. Starting at the origin"); + pose.x = 0.; + pose.y = 0.; + pose.theta = 0.; + } + else + { + pose.x = read_pose[0]; + pose.y = read_pose[1]; + pose.theta = read_pose[2]; + } } else { - pose.x = read_pose[0]; - pose.y = read_pose[1]; - pose.theta = read_pose[2]; + nh_.getParam("map_start_at_dock", start_at_dock); } - } - else - { - nh_.getParam("map_start_at_dock", start_at_dock); + + return true; } - return true; + return false; } - return false; -} - -/*****************************************************************************/ + /*****************************************************************************/ karto::LaserRangeFinder* SlamToolbox::getLaser(const sensor_msgs::LaserScan::ConstPtr& scan) -/*****************************************************************************/ -{ + /*****************************************************************************/ + { const std::string& frame = scan->header.frame_id; if(lasers_.find(frame) == lasers_.end()) - { - try { - lasers_[frame] = laser_assistant_->toLaserMetadata(*scan); - dataset_->Add(lasers_[frame].getLaser(), true); - } + try + { + lasers_[frame] = laser_assistant_->toLaserMetadata(*scan); + dataset_->Add(lasers_[frame].getLaser(), true); + } catch (tf2::TransformException& e) - { - ROS_ERROR("Failed to compute laser pose, aborting initialization (%s)", - e.what()); - return nullptr; + { + ROS_ERROR("Failed to compute laser pose, aborting initialization (%s)", + e.what()); + return nullptr; + } } - } - return lasers_[frame].getLaser(); -} + return lasers_[frame].getLaser(); + } -/*****************************************************************************/ -bool SlamToolbox::updateMap() -/*****************************************************************************/ -{ - if (sst_.getNumSubscribers() == 0) + /*****************************************************************************/ + bool SlamToolbox::updateMap() + /*****************************************************************************/ { - return true; - } - boost::mutex::scoped_lock lock(smapper_mutex_); + if (sst_.getNumSubscribers() == 0) + { + return true; + } + boost::mutex::scoped_lock lock(smapper_mutex_); karto::OccupancyGrid* occ_grid = smapper_->getOccupancyGrid(resolution_); if(!occ_grid) - { - return false; - } + { + return false; + } - vis_utils::toNavMap(occ_grid, map_.map); + vis_utils::toNavMap(occ_grid, map_.map); - // publish map as current - map_.map.header.stamp = ros::Time::now(); - sst_.publish(map_.map); - sstm_.publish(map_.map.info); - - delete occ_grid; - occ_grid = nullptr; - return true; -} + // publish map as current + map_.map.header.stamp = ros::Time::now(); + sst_.publish(map_.map); + sstm_.publish(map_.map.info); -/*****************************************************************************/ -tf2::Stamped SlamToolbox::setTransformFromPoses( + delete occ_grid; + occ_grid = nullptr; + return true; + } + + /*****************************************************************************/ + tf2::Stamped SlamToolbox::setTransformFromPoses( const karto::Pose2& corrected_pose, const karto::Pose2& karto_pose, const ros::Time& t, const bool& update_reprocessing_transform) -/*****************************************************************************/ -{ - // Compute the map->odom transform - tf2::Stamped odom_to_map; + /*****************************************************************************/ + { + // Compute the map->odom transform + tf2::Stamped odom_to_map; tf2::Quaternion q(0.,0.,0.,1.0); - q.setRPY(0., 0., corrected_pose.GetHeading()); - tf2::Stamped base_to_map( - tf2::Transform(q, tf2::Vector3(corrected_pose.GetX(), + q.setRPY(0., 0., corrected_pose.GetHeading()); + tf2::Stamped base_to_map( + tf2::Transform(q, tf2::Vector3(corrected_pose.GetX(), corrected_pose.GetY(), 0.0)).inverse(), t, base_frame_); - try - { - geometry_msgs::TransformStamped base_to_map_msg, odom_to_map_msg; - tf2::convert(base_to_map, base_to_map_msg); - odom_to_map_msg = tf_->transform(base_to_map_msg, odom_frame_); - tf2::convert(odom_to_map_msg, odom_to_map); - } + try + { + geometry_msgs::TransformStamped base_to_map_msg, odom_to_map_msg; + tf2::convert(base_to_map, base_to_map_msg); + odom_to_map_msg = tf_->transform(base_to_map_msg, odom_frame_); + tf2::convert(odom_to_map_msg, odom_to_map); + } catch(tf2::TransformException& e) - { - ROS_ERROR("Transform from base_link to odom failed: %s", e.what()); - return odom_to_map; - } + { + ROS_ERROR("Transform from base_link to odom failed: %s", e.what()); + return odom_to_map; + } - // if we're continuing a previous session, we need to - // estimate the homogenous transformation between the old and new - // odometry frames and transform the new session - // into the older session's frame - if (update_reprocessing_transform) - { - tf2::Transform odom_to_base_serialized = base_to_map.inverse(); + // if we're continuing a previous session, we need to + // estimate the homogenous transformation between the old and new + // odometry frames and transform the new session + // into the older session's frame + if (update_reprocessing_transform) + { + tf2::Transform odom_to_base_serialized = base_to_map.inverse(); tf2::Quaternion q1(0.,0.,0.,1.0); - q1.setRPY(0., 0., tf2::getYaw(odom_to_base_serialized.getRotation())); - odom_to_base_serialized.setRotation(q1); - tf2::Transform odom_to_base_current = smapper_->toTfPose(karto_pose); - reprocessing_transform_ = - odom_to_base_serialized * odom_to_base_current.inverse(); - } + q1.setRPY(0., 0., tf2::getYaw(odom_to_base_serialized.getRotation())); + odom_to_base_serialized.setRotation(q1); + tf2::Transform odom_to_base_current = smapper_->toTfPose(karto_pose); + reprocessing_transform_ = + odom_to_base_serialized * odom_to_base_current.inverse(); + } - // set map to odom for our transformation thread to publish - boost::mutex::scoped_lock lock(map_to_odom_mutex_); + // set map to odom for our transformation thread to publish + boost::mutex::scoped_lock lock(map_to_odom_mutex_); map_to_odom_ = tf2::Transform(tf2::Quaternion( odom_to_map.getRotation() ), tf2::Vector3( odom_to_map.getOrigin() ) ).inverse(); - return odom_to_map; -} + return odom_to_map; + } -/*****************************************************************************/ + /*****************************************************************************/ karto::LocalizedRangeScan* SlamToolbox::getLocalizedRangeScan( karto::LaserRangeFinder* laser, const sensor_msgs::LaserScan::ConstPtr& scan, karto::Pose2& karto_pose) -/*****************************************************************************/ -{ - // Create a vector of doubles for karto - std::vector readings = laser_utils::scanToReadings( - *scan, lasers_[scan->header.frame_id].isInverted()); + /*****************************************************************************/ + { + // Create a vector of doubles for karto + std::vector readings = laser_utils::scanToReadings( + *scan, lasers_[scan->header.frame_id].isInverted()); - // transform by the reprocessing transform - tf2::Transform pose_original = smapper_->toTfPose(karto_pose); - tf2::Transform tf_pose_transformed = reprocessing_transform_ * pose_original; - karto::Pose2 transformed_pose = smapper_->toKartoPose(tf_pose_transformed); + // transform by the reprocessing transform + tf2::Transform pose_original = smapper_->toTfPose(karto_pose); + tf2::Transform tf_pose_transformed = reprocessing_transform_ * pose_original; + karto::Pose2 transformed_pose = smapper_->toKartoPose(tf_pose_transformed); - // create localized range scan + // create localized range scan karto::LocalizedRangeScan* range_scan = new karto::LocalizedRangeScan( - laser->GetName(), readings); - range_scan->SetOdometricPose(transformed_pose); - range_scan->SetCorrectedPose(transformed_pose); - return range_scan; -} - -/*****************************************************************************/ -bool SlamToolbox::shouldProcessScan( + laser->GetName(), readings); + range_scan->SetOdometricPose(transformed_pose); + range_scan->SetCorrectedPose(transformed_pose); + return range_scan; + } + + /*****************************************************************************/ + bool SlamToolbox::shouldProcessScan( const sensor_msgs::LaserScan::ConstPtr& scan, const karto::Pose2& pose) -/*****************************************************************************/ -{ - static karto::Pose2 last_pose; - static ros::Time last_scan_time = ros::Time(0.); + /*****************************************************************************/ + { + static karto::Pose2 last_pose; + static ros::Time last_scan_time = ros::Time(0.); static const double dist_thresh_sq = smapper_->getMapper()->getParamMinimumTravelDistance()* - smapper_->getMapper()->getParamMinimumTravelDistance(); + smapper_->getMapper()->getParamMinimumTravelDistance(); - // we give it a pass on the first measurement to get the ball rolling - if (first_measurement_) - { - last_scan_time = scan->header.stamp; - last_pose = pose; - first_measurement_ = false; - return true; - } + // we give it a pass on the first measurement to get the ball rolling + if (first_measurement_) + { + last_scan_time = scan->header.stamp; + last_pose = pose; + first_measurement_ = false; + return true; + } - // we are in a paused mode, reject incomming information + // we are in a paused mode, reject incomming information if(isPaused(NEW_MEASUREMENTS)) - { - return false; - } + { + return false; + } - // throttled out - if ((scan->header.seq % throttle_scans_) != 0) - { - return false; - } + // throttled out + if ((scan->header.seq % throttle_scans_) != 0) + { + return false; + } - // not enough time - if (scan->header.stamp - last_scan_time < minimum_time_interval_) - { - return false; - } + // not enough time + if (scan->header.stamp - last_scan_time < minimum_time_interval_) + { + return false; + } - // check moved enough, within 10% for correction error - const double sq_dist_to_last_accepted_pose = last_pose.SquaredDistance(pose); + // check moved enough, within 10% for correction error + const double sq_dist_to_last_accepted_pose = last_pose.SquaredDistance(pose); if(sq_dist_to_last_accepted_pose < 0.8 * dist_thresh_sq || scan->header.seq < 5) - { - return false; - } + { + return false; + } - last_pose = pose; - last_scan_time = scan->header.stamp; + last_pose = pose; + last_scan_time = scan->header.stamp; - return true; -} + return true; + } -/*****************************************************************************/ + /*****************************************************************************/ karto::LocalizedRangeScan* SlamToolbox::addScan( karto::LaserRangeFinder* laser, PosedScan& scan_w_pose) -/*****************************************************************************/ -{ - return addScan(laser, scan_w_pose.scan, scan_w_pose.pose); -} + /*****************************************************************************/ + { + return addScan(laser, scan_w_pose.scan, scan_w_pose.pose); + } -/*****************************************************************************/ + /*****************************************************************************/ karto::LocalizedRangeScan* SlamToolbox::addScan( karto::LaserRangeFinder* laser, const sensor_msgs::LaserScan::ConstPtr& scan, karto::Pose2& karto_pose) -/*****************************************************************************/ -{ - // get our localized range scan + /*****************************************************************************/ + { + // get our localized range scan karto::LocalizedRangeScan* range_scan = getLocalizedRangeScan( - laser, scan, karto_pose); + laser, scan, karto_pose); - // Add the localized range scan to the smapper - boost::mutex::scoped_lock lock(smapper_mutex_); - bool processed = false, update_reprocessing_transform = false; + // Add the localized range scan to the smapper + boost::mutex::scoped_lock lock(smapper_mutex_); + bool processed = false, update_reprocessing_transform = false; - if (processor_type_ == PROCESS) - { - processed = smapper_->getMapper()->Process(range_scan); - } - else if (processor_type_ == PROCESS_FIRST_NODE) - { - processed = smapper_->getMapper()->ProcessAtDock(range_scan); - 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_) - { - ROS_ERROR("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); - update_reprocessing_transform = true; - processor_type_ = PROCESS; - } - else - { - ROS_FATAL("SlamToolbox: No valid processor type set! Exiting."); - exit(-1); - } + if (processor_type_ == PROCESS) + { + processed = smapper_->getMapper()->Process(range_scan); + } + else if (processor_type_ == PROCESS_FIRST_NODE) + { + processed = smapper_->getMapper()->ProcessAtDock(range_scan); + 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_) + { + ROS_ERROR("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); + update_reprocessing_transform = true; + processor_type_ = PROCESS; + } + else + { + ROS_FATAL("SlamToolbox: No valid processor type set! Exiting."); + exit(-1); + } - // if successfully processed, create odom to map transformation - // and add our scan to storage + // if successfully processed, create odom to map transformation + // and add our scan to storage if(processed) - { - if (enable_interactive_mode_) { - scan_holder_->addScan(*scan); + if (enable_interactive_mode_) + { + scan_holder_->addScan(*scan); + } + + setTransformFromPoses(range_scan->GetCorrectedPose(), karto_pose, + scan->header.stamp, update_reprocessing_transform); + dataset_->Add(range_scan); + } + else + { + delete range_scan; + range_scan = nullptr; } - setTransformFromPoses(range_scan->GetCorrectedPose(), karto_pose, - scan->header.stamp, update_reprocessing_transform); - dataset_->Add(range_scan); - } - else - { - delete range_scan; - range_scan = nullptr; + return range_scan; } - return range_scan; -} - -/*****************************************************************************/ -bool SlamToolbox::mapCallback( - nav_msgs::GetMap::Request &req, - nav_msgs::GetMap::Response &res) -/*****************************************************************************/ -{ - if(map_.map.info.width && map_.map.info.height) - { - boost::mutex::scoped_lock lock(smapper_mutex_); - res = map_; - return true; - } - else + /*****************************************************************************/ + bool SlamToolbox::mapCallback( + nav_msgs::GetMap::Request &req, + nav_msgs::GetMap::Response &res) + /*****************************************************************************/ { - return false; + if(map_.map.info.width && map_.map.info.height) + { + boost::mutex::scoped_lock lock(smapper_mutex_); + res = map_; + return true; + } + else + { + return false; + } } -} -/*****************************************************************************/ -bool SlamToolbox::pauseNewMeasurementsCallback( + /*****************************************************************************/ + bool SlamToolbox::pauseNewMeasurementsCallback( slam_toolbox_msgs::Pause::Request& req, slam_toolbox_msgs::Pause::Response& resp) -/*****************************************************************************/ -{ - bool curr_state = isPaused(NEW_MEASUREMENTS); - state_.set(NEW_MEASUREMENTS, !curr_state); + /*****************************************************************************/ + { + bool curr_state = isPaused(NEW_MEASUREMENTS); + state_.set(NEW_MEASUREMENTS, !curr_state); - nh_.setParam("paused_new_measurements", !curr_state); - ROS_INFO("SlamToolbox: Toggled to %s", + nh_.setParam("paused_new_measurements", !curr_state); + ROS_INFO("SlamToolbox: Toggled to %s", !curr_state ? "pause taking new measurements." : "actively taking new measurements."); - resp.status = true; - return true; -} + resp.status = true; + return true; + } -/*****************************************************************************/ + /*****************************************************************************/ bool SlamToolbox::isPaused(const PausedApplication& app) -/*****************************************************************************/ -{ - return state_.get(app); -} + /*****************************************************************************/ + { + return state_.get(app); + } -/*****************************************************************************/ -bool SlamToolbox::serializePoseGraphCallback( + /*****************************************************************************/ + bool SlamToolbox::serializePoseGraphCallback( slam_toolbox_msgs::SerializePoseGraph::Request &req, - slam_toolbox_msgs::SerializePoseGraph::Response &resp) -/*****************************************************************************/ -{ - std::string filename = req.filename; - - // if we're inside the snap, we need to write to commonly accessible space - if (snap_utils::isInSnap()) + slam_toolbox_msgs::SerializePoseGraph::Response &resp) + /*****************************************************************************/ { - filename = snap_utils::getSnapPath() + std::string("/") + filename; - } + std::string filename = req.filename; - boost::mutex::scoped_lock lock(smapper_mutex_); - serialization::write(filename, *smapper_->getMapper(), *dataset_); - return true; -} + // if we're inside the snap, we need to write to commonly accessible space + if (snap_utils::isInSnap()) + { + filename = snap_utils::getSnapPath() + std::string("/") + filename; + } -/*****************************************************************************/ -void SlamToolbox::loadSerializedPoseGraph( + boost::mutex::scoped_lock lock(smapper_mutex_); + serialization::write(filename, *smapper_->getMapper(), *dataset_); + return true; + } + + /*****************************************************************************/ + void SlamToolbox::loadSerializedPoseGraph( std::unique_ptr& mapper, std::unique_ptr& dataset) -/*****************************************************************************/ -{ - boost::mutex::scoped_lock lock(smapper_mutex_); + /*****************************************************************************/ + { + boost::mutex::scoped_lock lock(smapper_mutex_); - solver_->Reset(); + solver_->Reset(); - // add the nodes and constraints to the optimizer - VerticeMap mapper_vertices = mapper->GetGraph()->GetVertices(); - VerticeMap::iterator vertex_map_it = mapper_vertices.begin(); + // add the nodes and constraints to the optimizer + VerticeMap mapper_vertices = mapper->GetGraph()->GetVertices(); + VerticeMap::iterator vertex_map_it = mapper_vertices.begin(); for(vertex_map_it; vertex_map_it != mapper_vertices.end(); ++vertex_map_it) - { - ScanMap::iterator vertex_it = vertex_map_it->second.begin(); - for(vertex_it; vertex_it != vertex_map_it->second.end(); ++vertex_it) { - if (vertex_it->second != nullptr) + ScanMap::iterator vertex_it = vertex_map_it->second.begin(); + for(vertex_it; vertex_it != vertex_map_it->second.end(); ++vertex_it) { - solver_->AddNode(vertex_it->second); + if (vertex_it->second != nullptr) + { + solver_->AddNode(vertex_it->second); + } } } - } - EdgeVector mapper_edges = mapper->GetGraph()->GetEdges(); - EdgeVector::iterator edges_it = mapper_edges.begin(); + EdgeVector mapper_edges = mapper->GetGraph()->GetEdges(); + EdgeVector::iterator edges_it = mapper_edges.begin(); for( edges_it; edges_it != mapper_edges.end(); ++edges_it) - { - if (*edges_it != nullptr) { - solver_->AddConstraint(*edges_it); + if (*edges_it != nullptr) + { + solver_->AddConstraint(*edges_it); + } } - } - mapper->SetScanSolver(solver_.get()); + mapper->SetScanSolver(solver_.get()); - // move the memory to our working dataset - smapper_->setMapper(mapper.release()); - smapper_->configure(nh_); - dataset_.reset(dataset.release()); + // move the memory to our working dataset + smapper_->setMapper(mapper.release()); + smapper_->configure(nh_); + dataset_.reset(dataset.release()); + closure_assistant_->setMapper(smapper_->getMapper()); - if (!smapper_->getMapper()) - { - ROS_FATAL("loadSerializedPoseGraph: Could not properly load " - "a valid mapping object. Did you modify something by hand?"); - exit(-1); - } + if (!smapper_->getMapper()) + { + ROS_FATAL("loadSerializedPoseGraph: Could not properly load " + "a valid mapping object. Did you modify something by hand?"); + exit(-1); + } - if (dataset_->GetLasers().size() < 1) - { - ROS_FATAL("loadSerializedPoseGraph: Cannot deserialize " - "dataset with no laser objects."); - exit(-1); - } + if (dataset_->GetLasers().size() < 1) + { + ROS_FATAL("loadSerializedPoseGraph: Cannot deserialize " + "dataset with no laser objects."); + exit(-1); + } - // create a current laser sensor + // create a current laser sensor karto::LaserRangeFinder* laser = dynamic_cast( - dataset_->GetLasers()[0]); + dataset_->GetLasers()[0]); karto::Sensor* pSensor = dynamic_cast(laser); - if (pSensor) - { - karto::SensorManager::GetInstance()->RegisterSensor(pSensor); - - while (ros::ok()) + if (pSensor) { - ROS_INFO("Waiting for incoming scan to get metadata..."); - boost::shared_ptr scan = - ros::topic::waitForMessage( - scan_topic_, ros::Duration(1.0)); - if (scan) + karto::SensorManager::GetInstance()->RegisterSensor(pSensor); + + while (ros::ok()) { - ROS_INFO("Got scan!"); - try + ROS_INFO("Waiting for incoming scan to get metadata..."); + boost::shared_ptr scan = + ros::topic::waitForMessage( + scan_topic_, ros::Duration(1.0)); + if (scan) { - lasers_[scan->header.frame_id] = - laser_assistant_->toLaserMetadata(*scan); - break; - } + ROS_INFO("Got scan!"); + try + { + lasers_[scan->header.frame_id] = + laser_assistant_->toLaserMetadata(*scan); + break; + } catch (tf2::TransformException& e) - { - ROS_ERROR("Failed to compute laser pose, aborting continue mapping (%s)", - e.what()); - exit(-1); + { + ROS_ERROR("Failed to compute laser pose, aborting continue mapping (%s)", + e.what()); + exit(-1); + } } } } - } - else - { - ROS_ERROR("Invalid sensor pointer in dataset. Unable to register sensor."); - } + else + { + ROS_ERROR("Invalid sensor pointer in dataset. Unable to register sensor."); + } - solver_->Compute(); + solver_->Compute(); - return; -} + return; + } -/*****************************************************************************/ -bool SlamToolbox::deserializePoseGraphCallback( + /*****************************************************************************/ + bool SlamToolbox::deserializePoseGraphCallback( slam_toolbox_msgs::DeserializePoseGraph::Request &req, - slam_toolbox_msgs::DeserializePoseGraph::Response &resp) -/*****************************************************************************/ -{ - if (req.match_type == slam_toolbox_msgs::DeserializePoseGraph::Request::UNSET) + slam_toolbox_msgs::DeserializePoseGraph::Response &resp) + /*****************************************************************************/ { - ROS_ERROR("Deserialization called without valid processor type set. " - "Undefined behavior!"); - return false; - } + if (req.match_type == slam_toolbox_msgs::DeserializePoseGraph::Request::UNSET) + { + ROS_ERROR("Deserialization called without valid processor type set. " + "Undefined behavior!"); + return false; + } - std::string filename = req.filename; + std::string filename = req.filename; - if (filename.empty()) - { - ROS_WARN("No map file given!"); - return true; - } + if (filename.empty()) + { + ROS_WARN("No map file given!"); + return true; + } - // if we're inside the snap, we need to write to commonly accessible space - if (snap_utils::isInSnap()) - { - filename = snap_utils::getSnapPath() + std::string("/") + filename; - } + // if we're inside the snap, we need to write to commonly accessible space + if (snap_utils::isInSnap()) + { + filename = snap_utils::getSnapPath() + std::string("/") + filename; + } - std::unique_ptr dataset = std::make_unique(); - std::unique_ptr mapper = std::make_unique(); + std::unique_ptr dataset = std::make_unique(); + std::unique_ptr mapper = std::make_unique(); - if (!serialization::read(filename, *mapper, *dataset)) - { - ROS_ERROR("DeserializePoseGraph: Failed to read " + if (!serialization::read(filename, *mapper, *dataset)) + { + ROS_ERROR("DeserializePoseGraph: Failed to read " "file: %s.", filename.c_str()); - return true; - } - ROS_DEBUG("DeserializePoseGraph: Successfully read file."); + return true; + } + ROS_DEBUG("DeserializePoseGraph: Successfully read file."); - loadSerializedPoseGraph(mapper, dataset); - updateMap(); + loadSerializedPoseGraph(mapper, dataset); + updateMap(); - first_measurement_ = true; - boost::mutex::scoped_lock l(pose_mutex_); - switch (req.match_type) - { + first_measurement_ = true; + boost::mutex::scoped_lock l(pose_mutex_); + switch (req.match_type) + { case procType::START_AT_FIRST_NODE: processor_type_ = PROCESS_FIRST_NODE; break; case procType::START_AT_GIVEN_POSE: processor_type_ = PROCESS_NEAR_REGION; - process_near_pose_ = std::make_unique(req.initial_pose.x, - req.initial_pose.y, req.initial_pose.theta); + process_near_pose_ = std::make_unique(req.initial_pose.x, + req.initial_pose.y, req.initial_pose.theta); break; - case procType::LOCALIZE_AT_POSE: + case procType::LOCALIZE_AT_POSE: processor_type_ = PROCESS_LOCALIZATION; - process_near_pose_ = std::make_unique(req.initial_pose.x, - req.initial_pose.y, req.initial_pose.theta); + process_near_pose_ = std::make_unique(req.initial_pose.x, + req.initial_pose.y, req.initial_pose.theta); break; default: ROS_FATAL("Deserialization called without valid processor type set."); - } + } - return true; -} + return true; + } } // end namespace From 7449a346b96a1af7361f9e5621ca67a77a1da596 Mon Sep 17 00:00:00 2001 From: suryakanth Date: Fri, 11 Nov 2022 16:11:19 +0530 Subject: [PATCH 02/13] constraints --- slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h | 8 ++++++++ slam_toolbox/src/loop_closure_assistant.cpp | 7 +++---- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h b/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h index 6e538a05f..4fdcf6b28 100644 --- a/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -2056,6 +2056,14 @@ namespace karto { return m_pMapperSensorManager; } + /** + * Gets the localization vertices + * @return localization vertices + */ + const LocalizationScanVertices &GetLocalizationVertices() + { + return m_LocalizationScanVertices; + } /** * Tries to close a loop using the given scan with the scans from the given sensor diff --git a/slam_toolbox/src/loop_closure_assistant.cpp b/slam_toolbox/src/loop_closure_assistant.cpp index 9f88db97a..7623e5ac3 100644 --- a/slam_toolbox/src/loop_closure_assistant.cpp +++ b/slam_toolbox/src/loop_closure_assistant.cpp @@ -99,6 +99,7 @@ void LoopClosureAssistant::processInteractiveFeedback(const tf2::Quaternion q2(0.,0.,0.,1.0); q2.setEuler(0., 0., 3.14159); quat *= q1; + } quat *= q2; // interactive move @@ -243,7 +244,7 @@ void LoopClosureAssistant::processInteractiveFeedback(const visualization_msgs::Marker loc_edges_marker = getMarker("intra_loc_edges", localization_vertices.size() * 3); visualization_msgs::Marker inter_loc_edges_marker = getMarker("inter_loc_edges", localization_vertices.size() * 3); - geometry_msgs::Point prevSourcePoint, prevTargetPoint; + geometry_msgs::Point prevTargetPoint; for (const auto &edge : edges) { isInter = false; @@ -257,7 +258,6 @@ void LoopClosureAssistant::processInteractiveFeedback(const geometry_msgs::Point sourcePoint; sourcePoint.x = pose0.GetX(); sourcePoint.y = pose0.GetY(); - // if (prevTargetPoint == sourcePoint) int target_id = edge->GetTarget()->GetObject()->GetUniqueId(); const auto &pose1 = edge->GetTarget()->GetObject()->GetCorrectedPose(); @@ -271,10 +271,9 @@ void LoopClosureAssistant::processInteractiveFeedback(const } else { - color = getColor(1, 1, 0); // yello + color = getColor(1, 1, 0); //r,g,b } - prevSourcePoint = sourcePoint; prevTargetPoint = targetPoint; if (source_id >= first_localization_id || target_id >= first_localization_id) From 342cef058f49d0805f989ba652a427895591493d Mon Sep 17 00:00:00 2001 From: suryakanth Date: Fri, 11 Nov 2022 16:20:23 +0530 Subject: [PATCH 03/13] constraints visualization --- slam_toolbox/src/loop_closure_assistant.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/slam_toolbox/src/loop_closure_assistant.cpp b/slam_toolbox/src/loop_closure_assistant.cpp index 7623e5ac3..9f88db97a 100644 --- a/slam_toolbox/src/loop_closure_assistant.cpp +++ b/slam_toolbox/src/loop_closure_assistant.cpp @@ -99,7 +99,6 @@ void LoopClosureAssistant::processInteractiveFeedback(const tf2::Quaternion q2(0.,0.,0.,1.0); q2.setEuler(0., 0., 3.14159); quat *= q1; - } quat *= q2; // interactive move @@ -244,7 +243,7 @@ void LoopClosureAssistant::processInteractiveFeedback(const visualization_msgs::Marker loc_edges_marker = getMarker("intra_loc_edges", localization_vertices.size() * 3); visualization_msgs::Marker inter_loc_edges_marker = getMarker("inter_loc_edges", localization_vertices.size() * 3); - geometry_msgs::Point prevTargetPoint; + geometry_msgs::Point prevSourcePoint, prevTargetPoint; for (const auto &edge : edges) { isInter = false; @@ -258,6 +257,7 @@ void LoopClosureAssistant::processInteractiveFeedback(const geometry_msgs::Point sourcePoint; sourcePoint.x = pose0.GetX(); sourcePoint.y = pose0.GetY(); + // if (prevTargetPoint == sourcePoint) int target_id = edge->GetTarget()->GetObject()->GetUniqueId(); const auto &pose1 = edge->GetTarget()->GetObject()->GetCorrectedPose(); @@ -271,9 +271,10 @@ void LoopClosureAssistant::processInteractiveFeedback(const } else { - color = getColor(1, 1, 0); //r,g,b + color = getColor(1, 1, 0); // yello } + prevSourcePoint = sourcePoint; prevTargetPoint = targetPoint; if (source_id >= first_localization_id || target_id >= first_localization_id) From 131ba499b1971f37f3513d7fa4d509f0ab156b60 Mon Sep 17 00:00:00 2001 From: suryakanth Date: Sat, 12 Nov 2022 00:26:26 +0530 Subject: [PATCH 04/13] Reverted all unnecessary code reformatting --- .../slam_toolbox/loop_closure_assistant.hpp | 2 +- .../lib/karto_sdk/include/karto_sdk/Mapper.h | 1 - slam_toolbox/src/loop_closure_assistant.cpp | 415 +++---- slam_toolbox/src/slam_toolbox_common.cpp | 1094 ++++++++--------- 4 files changed, 756 insertions(+), 756 deletions(-) diff --git a/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp b/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp index d15c5cff1..a47ddd8c3 100644 --- a/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp +++ b/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp @@ -54,10 +54,10 @@ class LoopClosureAssistant bool interactiveModeCallback(slam_toolbox_msgs::ToggleInteractive::Request &req, slam_toolbox_msgs::ToggleInteractive::Response &resp); void moveNode(const int& id, const Eigen::Vector3d& pose); void addMovedNodes(const int& id, Eigen::Vector3d vec); + std_msgs::ColorRGBA getColor(float r, float g, float b); visualization_msgs::Marker getMarker( std::string name,int reserve); - std::unique_ptr tfB_; laser_utils::ScanHolder* scan_holder_; ros::Publisher scan_publisher_, marker_publisher_; diff --git a/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h b/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h index 4fdcf6b28..90a068540 100644 --- a/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -2064,7 +2064,6 @@ namespace karto { return m_LocalizationScanVertices; } - /** * Tries to close a loop using the given scan with the scans from the given sensor * @param pScan diff --git a/slam_toolbox/src/loop_closure_assistant.cpp b/slam_toolbox/src/loop_closure_assistant.cpp index 9f88db97a..81a10ffc6 100644 --- a/slam_toolbox/src/loop_closure_assistant.cpp +++ b/slam_toolbox/src/loop_closure_assistant.cpp @@ -21,144 +21,142 @@ namespace loop_closure_assistant { - /*****************************************************************************/ - LoopClosureAssistant::LoopClosureAssistant( +/*****************************************************************************/ +LoopClosureAssistant::LoopClosureAssistant( ros::NodeHandle& node, karto::Mapper* mapper, laser_utils::ScanHolder* scan_holder, PausedState& state, ProcessType & processor_type) - : mapper_(mapper), scan_holder_(scan_holder), - interactive_mode_(false), nh_(node), state_(state), - processor_type_(processor_type) - /*****************************************************************************/ - { - node.setParam("paused_processing", false); - tfB_ = std::make_unique(); - ssClear_manual_ = node.advertiseService("clear_changes", - &LoopClosureAssistant::clearChangesCallback, this); - ssLoopClosure_ = node.advertiseService("manual_loop_closure", - &LoopClosureAssistant::manualLoopClosureCallback, this); - scan_publisher_ = node.advertise( +: mapper_(mapper), scan_holder_(scan_holder), + interactive_mode_(false), nh_(node), state_(state), + processor_type_(processor_type) +/*****************************************************************************/ +{ + node.setParam("paused_processing", false); + tfB_ = std::make_unique(); + ssClear_manual_ = node.advertiseService("clear_changes", + &LoopClosureAssistant::clearChangesCallback, this); + ssLoopClosure_ = node.advertiseService("manual_loop_closure", + &LoopClosureAssistant::manualLoopClosureCallback, this); + scan_publisher_ = node.advertise( "karto_scan_visualization",10); - solver_ = mapper_->getScanSolver(); - interactive_server_ = - std::make_unique( + solver_ = mapper_->getScanSolver(); + interactive_server_ = + std::make_unique( "slam_toolbox","",true); - ssInteractive_ = node.advertiseService("toggle_interactive_mode", + ssInteractive_ = node.advertiseService("toggle_interactive_mode", &LoopClosureAssistant::interactiveModeCallback,this); - node.setParam("interactive_mode", interactive_mode_); - marker_publisher_ = node.advertise( + node.setParam("interactive_mode", interactive_mode_); + marker_publisher_ = node.advertise( "karto_graph_visualization",1); - node.param("map_frame", map_frame_, std::string("map")); - node.param("enable_interactive_mode", enable_interactive_mode_, false); - } - /*****************************************************************************/ - void LoopClosureAssistant::setMapper(karto::Mapper *mapper) - /*****************************************************************************/ - { - mapper_ = mapper; - } + node.param("map_frame", map_frame_, std::string("map")); + node.param("enable_interactive_mode", enable_interactive_mode_, false); +} - /*****************************************************************************/ +/*****************************************************************************/ void LoopClosureAssistant::processInteractiveFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) - /*****************************************************************************/ +/*****************************************************************************/ +{ + if (processor_type_ != PROCESS) { - if (processor_type_ != PROCESS) - { - ROS_ERROR_THROTTLE(5., - "Interactive mode is invalid outside processing mode."); - return; - } + ROS_ERROR_THROTTLE(5., + "Interactive mode is invalid outside processing mode."); + return; + } - const int id = std::stoi(feedback->marker_name, nullptr, 10) - 1; + const int id = std::stoi(feedback->marker_name, nullptr, 10) - 1; - // was depressed, something moved, and now released - if (feedback->event_type == - visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP && - feedback->mouse_point_valid) - { - addMovedNodes(id, Eigen::Vector3d(feedback->mouse_point.x, - feedback->mouse_point.y, tf2::getYaw(feedback->pose.orientation))); - } + // was depressed, something moved, and now released + if (feedback->event_type == + visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP && + feedback->mouse_point_valid) + { + addMovedNodes(id, Eigen::Vector3d(feedback->mouse_point.x, + feedback->mouse_point.y, tf2::getYaw(feedback->pose.orientation))); + } - // is currently depressed, being moved before release - if (feedback->event_type == - visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE) - { - // get scan - sensor_msgs::LaserScan scan = scan_holder_->getCorrectedScan(id); + // is currently depressed, being moved before release + if (feedback->event_type == + visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE) + { + // get scan + sensor_msgs::LaserScan scan = scan_holder_->getCorrectedScan(id); - // get correct orientation + // get correct orientation tf2::Quaternion quat(0.,0.,0.,1.0), msg_quat(0.,0.,0.,1.0); - double node_yaw, first_node_yaw; - solver_->GetNodeOrientation(id, node_yaw); - solver_->GetNodeOrientation(0, first_node_yaw); + double node_yaw, first_node_yaw; + solver_->GetNodeOrientation(id, node_yaw); + solver_->GetNodeOrientation(0, first_node_yaw); tf2::Quaternion q1(0.,0.,0.,1.0); - q1.setEuler(0., 0., node_yaw - 3.14159); + q1.setEuler(0., 0., node_yaw - 3.14159); tf2::Quaternion q2(0.,0.,0.,1.0); - q2.setEuler(0., 0., 3.14159); - quat *= q1; - quat *= q2; - - // interactive move - tf2::convert(feedback->pose.orientation, msg_quat); - quat *= msg_quat; - quat.normalize(); - - // create correct transform - tf2::Transform transform; - transform.setOrigin(tf2::Vector3(feedback->pose.position.x, - feedback->pose.position.y, 0.)); - transform.setRotation(quat); - - // publish the scan visualization with transform - geometry_msgs::TransformStamped msg; - tf2::convert(transform, msg.transform); - msg.child_frame_id = "karto_scan_visualization"; - msg.header.frame_id = feedback->header.frame_id; - msg.header.stamp = ros::Time::now(); - tfB_->sendTransform(msg); - - scan.header.frame_id = "karto_scan_visualization"; - scan.header.stamp = ros::Time::now(); - scan_publisher_.publish(scan); - } - } - - std_msgs::ColorRGBA LoopClosureAssistant::getColor(float r, float g, float b) - { - std_msgs::ColorRGBA color; - color.r = r; - color.g = g; - color.b = b; - return color; - } - - visualization_msgs::Marker LoopClosureAssistant::getMarker(std::string name, int reserve) - { - visualization_msgs::Marker edges_marker; - edges_marker.header.frame_id = map_frame_; - edges_marker.header.stamp = ros::Time::now(); - edges_marker.id = 0; - edges_marker.ns = name; - edges_marker.action = visualization_msgs::Marker::ADD; - edges_marker.type = visualization_msgs::Marker::LINE_LIST; - edges_marker.pose.orientation.w = 1; - edges_marker.scale.x = 0.01; - edges_marker.color.r = 0.0; - edges_marker.color.g = 0.0; - edges_marker.color.b = 0.0; - edges_marker.color.a = 1; - edges_marker.lifetime = ros::Duration(0.0); - edges_marker.points.reserve(reserve); - return edges_marker; + q2.setEuler(0., 0., 3.14159); + quat *= q1; + quat *= q2; + + // interactive move + tf2::convert(feedback->pose.orientation, msg_quat); + quat *= msg_quat; + quat.normalize(); + + // create correct transform + tf2::Transform transform; + transform.setOrigin(tf2::Vector3(feedback->pose.position.x, + feedback->pose.position.y, 0.)); + transform.setRotation(quat); + + // publish the scan visualization with transform + geometry_msgs::TransformStamped msg; + tf2::convert(transform, msg.transform); + msg.child_frame_id = "karto_scan_visualization"; + msg.header.frame_id = feedback->header.frame_id; + msg.header.stamp = ros::Time::now(); + tfB_->sendTransform(msg); + + scan.header.frame_id = "karto_scan_visualization"; + scan.header.stamp = ros::Time::now(); + scan_publisher_.publish(scan); } +} +/*****************************************************************************/ +void LoopClosureAssistant::setMapper(karto::Mapper *mapper) +/*****************************************************************************/ +{ + mapper_ = mapper; +} - /*****************************************************************************/ - void LoopClosureAssistant::publishGraph() - /*****************************************************************************/ - { +std_msgs::ColorRGBA LoopClosureAssistant::getColor(float r, float g, float b) +{ + std_msgs::ColorRGBA color; + color.r = r; + color.g = g; + color.b = b; + return color; +} +visualization_msgs::Marker LoopClosureAssistant::getMarker(std::string name, int reserve) +{ + visualization_msgs::Marker edges_marker; + edges_marker.header.frame_id = map_frame_; + edges_marker.header.stamp = ros::Time::now(); + edges_marker.id = 0; + edges_marker.ns = name; + edges_marker.action = visualization_msgs::Marker::ADD; + edges_marker.type = visualization_msgs::Marker::LINE_LIST; + edges_marker.pose.orientation.w = 1; + edges_marker.scale.x = 0.01; + edges_marker.color.r = 0.0; + edges_marker.color.g = 0.0; + edges_marker.color.b = 0.0; + edges_marker.color.a = 1; + edges_marker.lifetime = ros::Duration(0.0); + edges_marker.points.reserve(reserve); + return edges_marker; +} +/*****************************************************************************/ +void LoopClosureAssistant::publishGraph() +/*****************************************************************************/ +{ interactive_server_->clear(); std::unordered_map* graph = solver_->getGraph(); @@ -313,134 +311,137 @@ void LoopClosureAssistant::processInteractiveFeedback(const } } } + marray.markers.push_back(edges_marker_inter); marray.markers.push_back(edges_marker); + if(inter_loc_edges_marker.points.size() > 0) // to avoid error when mapping marray.markers.push_back(inter_loc_edges_marker); + if (loc_edges_marker.points.size() > 0) marray.markers.push_back(loc_edges_marker); // if disabled, clears out old markers interactive_server_->applyChanges(); marker_publisher_.publish(marray); return; - } +} - /*****************************************************************************/ - bool LoopClosureAssistant::manualLoopClosureCallback( +/*****************************************************************************/ +bool LoopClosureAssistant::manualLoopClosureCallback( slam_toolbox_msgs::LoopClosure::Request& req, slam_toolbox_msgs::LoopClosure::Response& resp) - /*****************************************************************************/ - { +/*****************************************************************************/ +{ if(!enable_interactive_mode_) - { - ROS_WARN("Called manual loop closure" - " with interactive mode disabled. Ignoring."); - return false; - } + { + ROS_WARN("Called manual loop closure" + " with interactive mode disabled. Ignoring."); + return false; + } - { - boost::mutex::scoped_lock lock(moved_nodes_mutex_); + { + boost::mutex::scoped_lock lock(moved_nodes_mutex_); - if (moved_nodes_.size() == 0) - { - ROS_WARN("No moved nodes to attempt manual loop closure."); - return true; - } + if (moved_nodes_.size() == 0) + { + ROS_WARN("No moved nodes to attempt manual loop closure."); + return true; + } - ROS_INFO("LoopClosureAssistant: Attempting to manual " + ROS_INFO("LoopClosureAssistant: Attempting to manual " "loop close with %i moved nodes.", (int)moved_nodes_.size()); - // for each in node map - std::map::const_iterator it = moved_nodes_.begin(); - for (it; it != moved_nodes_.end(); ++it) - { - moveNode(it->first, + // for each in node map + std::map::const_iterator it = moved_nodes_.begin(); + for (it; it != moved_nodes_.end(); ++it) + { + moveNode(it->first, Eigen::Vector3d(it->second(0),it->second(1), it->second(2))); - } } + } - // optimize - mapper_->CorrectPoses(); + // optimize + mapper_->CorrectPoses(); - // update visualization and clear out nodes completed - publishGraph(); - clearMovedNodes(); - return true; - } + // update visualization and clear out nodes completed + publishGraph(); + clearMovedNodes(); + return true; +} - /*****************************************************************************/ - bool LoopClosureAssistant::interactiveModeCallback( +/*****************************************************************************/ +bool LoopClosureAssistant::interactiveModeCallback( slam_toolbox_msgs::ToggleInteractive::Request &req, - slam_toolbox_msgs::ToggleInteractive::Response &resp) - /*****************************************************************************/ - { + slam_toolbox_msgs::ToggleInteractive::Response &resp) +/*****************************************************************************/ +{ if(!enable_interactive_mode_) - { - ROS_WARN("Called toggle interactive mode with " - "interactive mode disabled. Ignoring."); - return false; - } + { + ROS_WARN("Called toggle interactive mode with " + "interactive mode disabled. Ignoring."); + return false; + } - bool interactive_mode; - { - boost::mutex::scoped_lock lock_i(interactive_mutex_); - interactive_mode_ = !interactive_mode_; - interactive_mode = interactive_mode_; - nh_.setParam("interactive_mode", interactive_mode_); - } + bool interactive_mode; + { + boost::mutex::scoped_lock lock_i(interactive_mutex_); + interactive_mode_ = !interactive_mode_; + interactive_mode = interactive_mode_; + nh_.setParam("interactive_mode", interactive_mode_); + } - ROS_INFO("SlamToolbox: Toggling %s interactive mode.", - interactive_mode ? "on" : "off"); - publishGraph(); - clearMovedNodes(); + ROS_INFO("SlamToolbox: Toggling %s interactive mode.", + interactive_mode ? "on" : "off"); + publishGraph(); + clearMovedNodes(); - // set state so we don't overwrite changes in rviz while loop closing - state_.set(PROCESSING, interactive_mode); - state_.set(VISUALIZING_GRAPH, interactive_mode); - nh_.setParam("paused_processing", interactive_mode); - return true; - } + // set state so we don't overwrite changes in rviz while loop closing + state_.set(PROCESSING, interactive_mode); + state_.set(VISUALIZING_GRAPH, interactive_mode); + nh_.setParam("paused_processing", interactive_mode); + return true; +} - /*****************************************************************************/ - void LoopClosureAssistant::moveNode( +/*****************************************************************************/ +void LoopClosureAssistant::moveNode( const int& id, const Eigen::Vector3d& pose) - /*****************************************************************************/ - { - solver_->ModifyNode(id, pose); - } +/*****************************************************************************/ +{ + solver_->ModifyNode(id, pose); +} - /*****************************************************************************/ - bool LoopClosureAssistant::clearChangesCallback( +/*****************************************************************************/ +bool LoopClosureAssistant::clearChangesCallback( slam_toolbox_msgs::Clear::Request& req, slam_toolbox_msgs::Clear::Response& resp) - /*****************************************************************************/ - { +/*****************************************************************************/ +{ if(!enable_interactive_mode_) - { - ROS_WARN("Called Clear changes with interactive mode disabled. Ignoring."); - return false; - } - - ROS_INFO("LoopClosureAssistant: Clearing manual loop closure nodes."); - publishGraph(); - clearMovedNodes(); - return true; + { + ROS_WARN("Called Clear changes with interactive mode disabled. Ignoring."); + return false; } - /*****************************************************************************/ + ROS_INFO("LoopClosureAssistant: Clearing manual loop closure nodes."); + publishGraph(); + clearMovedNodes(); + return true; +} + +/*****************************************************************************/ void LoopClosureAssistant::clearMovedNodes() - /*****************************************************************************/ - { - boost::mutex::scoped_lock lock(moved_nodes_mutex_); - moved_nodes_.clear(); - } +/*****************************************************************************/ +{ + boost::mutex::scoped_lock lock(moved_nodes_mutex_); + moved_nodes_.clear(); +} - /*****************************************************************************/ +/*****************************************************************************/ void LoopClosureAssistant::addMovedNodes(const int& id, Eigen::Vector3d vec) - /*****************************************************************************/ - { - ROS_INFO("LoopClosureAssistant: Node %i new manual loop closure " +/*****************************************************************************/ +{ + ROS_INFO("LoopClosureAssistant: Node %i new manual loop closure " "pose has been recorded.",id); - boost::mutex::scoped_lock lock(moved_nodes_mutex_); - moved_nodes_[id] = vec; - } + boost::mutex::scoped_lock lock(moved_nodes_mutex_); + moved_nodes_[id] = vec; +} } // end namespace \ No newline at end of file diff --git a/slam_toolbox/src/slam_toolbox_common.cpp b/slam_toolbox/src/slam_toolbox_common.cpp index 0e217ac41..10a6be866 100644 --- a/slam_toolbox/src/slam_toolbox_common.cpp +++ b/slam_toolbox/src/slam_toolbox_common.cpp @@ -24,742 +24,742 @@ namespace slam_toolbox { - /*****************************************************************************/ - SlamToolbox::SlamToolbox(ros::NodeHandle& nh) - : solver_loader_("slam_toolbox", "karto::ScanSolver"), - processor_type_(PROCESS), - first_measurement_(true), - nh_(nh), - process_near_pose_(nullptr) - /*****************************************************************************/ - { - smapper_ = std::make_unique(); - dataset_ = std::make_unique(); - - setParams(nh_); - setROSInterfaces(nh_); - setSolver(nh_); - - laser_assistant_ = std::make_unique( - nh_, tf_.get(), base_frame_); - pose_helper_ = std::make_unique( - tf_.get(), base_frame_, odom_frame_); - scan_holder_ = std::make_unique(lasers_); - map_saver_ = std::make_unique(nh_, map_name_); - closure_assistant_ = - std::make_unique( - nh_, smapper_->getMapper(), scan_holder_.get(), state_, processor_type_); - - reprocessing_transform_.setIdentity(); - - double transform_publish_period; - nh_.param("transform_publish_period", transform_publish_period, 0.05); - threads_.push_back(std::make_unique( - boost::bind(&SlamToolbox::publishTransformLoop, - this, transform_publish_period))); - threads_.push_back(std::make_unique( - boost::bind(&SlamToolbox::publishVisualizations, this))); - } - - /*****************************************************************************/ - SlamToolbox::~SlamToolbox() - /*****************************************************************************/ - { +/*****************************************************************************/ +SlamToolbox::SlamToolbox(ros::NodeHandle& nh) +: solver_loader_("slam_toolbox", "karto::ScanSolver"), + processor_type_(PROCESS), + first_measurement_(true), + nh_(nh), + process_near_pose_(nullptr) +/*****************************************************************************/ +{ + smapper_ = std::make_unique(); + dataset_ = std::make_unique(); + + setParams(nh_); + setROSInterfaces(nh_); + setSolver(nh_); + + laser_assistant_ = std::make_unique( + nh_, tf_.get(), base_frame_); + pose_helper_ = std::make_unique( + tf_.get(), base_frame_, odom_frame_); + scan_holder_ = std::make_unique(lasers_); + map_saver_ = std::make_unique(nh_, map_name_); + closure_assistant_ = + std::make_unique( + nh_, smapper_->getMapper(), scan_holder_.get(), state_, processor_type_); + + reprocessing_transform_.setIdentity(); + + double transform_publish_period; + nh_.param("transform_publish_period", transform_publish_period, 0.05); + threads_.push_back(std::make_unique( + boost::bind(&SlamToolbox::publishTransformLoop, + this, transform_publish_period))); + threads_.push_back(std::make_unique( + boost::bind(&SlamToolbox::publishVisualizations, this))); +} + +/*****************************************************************************/ +SlamToolbox::~SlamToolbox() +/*****************************************************************************/ +{ for (int i=0; i != threads_.size(); i++) - { - threads_[i]->join(); - } - - smapper_.reset(); - dataset_.reset(); - closure_assistant_.reset(); - map_saver_.reset(); - pose_helper_.reset(); - laser_assistant_.reset(); - scan_holder_.reset(); + { + threads_[i]->join(); } - /*****************************************************************************/ + smapper_.reset(); + dataset_.reset(); + closure_assistant_.reset(); + map_saver_.reset(); + pose_helper_.reset(); + laser_assistant_.reset(); + scan_holder_.reset(); +} + +/*****************************************************************************/ void SlamToolbox::setSolver(ros::NodeHandle& private_nh_) - /*****************************************************************************/ - { - // Set solver to be used in loop closure - std::string solver_plugin; +/*****************************************************************************/ +{ + // Set solver to be used in loop closure + std::string solver_plugin; if(!private_nh_.getParam("solver_plugin", solver_plugin)) - { - ROS_WARN("unable to find requested solver plugin, defaulting to SPA"); - solver_plugin = "solver_plugins::CeresSolver"; - } - try - { - solver_ = solver_loader_.createInstance(solver_plugin); - ROS_INFO("Using plugin %s", solver_plugin.c_str()); - } + { + ROS_WARN("unable to find requested solver plugin, defaulting to SPA"); + solver_plugin = "solver_plugins::CeresSolver"; + } + try + { + solver_ = solver_loader_.createInstance(solver_plugin); + ROS_INFO("Using plugin %s", solver_plugin.c_str()); + } catch (const pluginlib::PluginlibException& ex) - { - ROS_FATAL("Failed to create %s, is it registered and built? Exception: %s.", - solver_plugin.c_str(), ex.what()); - exit(1); - } - smapper_->getMapper()->SetScanSolver(solver_.get()); + { + ROS_FATAL("Failed to create %s, is it registered and built? Exception: %s.", + solver_plugin.c_str(), ex.what()); + exit(1); } + smapper_->getMapper()->SetScanSolver(solver_.get()); +} - /*****************************************************************************/ +/*****************************************************************************/ void SlamToolbox::setParams(ros::NodeHandle& private_nh) - /*****************************************************************************/ - { - map_to_odom_.setIdentity(); - private_nh.param("odom_frame", odom_frame_, std::string("odom")); - private_nh.param("map_frame", map_frame_, std::string("map")); - private_nh.param("base_frame", base_frame_, std::string("base_footprint")); - private_nh.param("resolution", resolution_, 0.05); - private_nh.param("map_name", map_name_, std::string("/map")); - private_nh.param("scan_topic", scan_topic_, std::string("/scan")); - private_nh.param("throttle_scans", throttle_scans_, 1); - private_nh.param("enable_interactive_mode", enable_interactive_mode_, false); - - double tmp_val; - private_nh.param("transform_timeout", tmp_val, 0.2); - transform_timeout_ = ros::Duration(tmp_val); - private_nh.param("tf_buffer_duration", tmp_val, 30.); - tf_buffer_dur_ = ros::Duration(tmp_val); - private_nh.param("minimum_time_interval", tmp_val, 0.5); - minimum_time_interval_ = ros::Duration(tmp_val); - - bool debug = false; - if (private_nh.getParam("debug_logging", debug) && debug) +/*****************************************************************************/ +{ + map_to_odom_.setIdentity(); + private_nh.param("odom_frame", odom_frame_, std::string("odom")); + private_nh.param("map_frame", map_frame_, std::string("map")); + private_nh.param("base_frame", base_frame_, std::string("base_footprint")); + private_nh.param("resolution", resolution_, 0.05); + private_nh.param("map_name", map_name_, std::string("/map")); + private_nh.param("scan_topic", scan_topic_, std::string("/scan")); + private_nh.param("throttle_scans", throttle_scans_, 1); + private_nh.param("enable_interactive_mode", enable_interactive_mode_, false); + + double tmp_val; + private_nh.param("transform_timeout", tmp_val, 0.2); + transform_timeout_ = ros::Duration(tmp_val); + private_nh.param("tf_buffer_duration", tmp_val, 30.); + tf_buffer_dur_ = ros::Duration(tmp_val); + private_nh.param("minimum_time_interval", tmp_val, 0.5); + minimum_time_interval_ = ros::Duration(tmp_val); + + bool debug = false; + if (private_nh.getParam("debug_logging", debug) && debug) + { + if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, + ros::console::levels::Debug)) { - if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, - ros::console::levels::Debug)) - { - ros::console::notifyLoggerLevelsChanged(); - } + ros::console::notifyLoggerLevelsChanged(); } - - smapper_->configure(private_nh); - private_nh.setParam("paused_new_measurements", false); } - /*****************************************************************************/ + smapper_->configure(private_nh); + private_nh.setParam("paused_new_measurements", false); +} + +/*****************************************************************************/ void SlamToolbox::setROSInterfaces(ros::NodeHandle& node) - /*****************************************************************************/ - { - tf_ = std::make_unique(ros::Duration(tf_buffer_dur_)); - tfL_ = std::make_unique(*tf_); - tfB_ = std::make_unique(); - sst_ = node.advertise(map_name_, 1, true); - sstm_ = node.advertise(map_name_ + "_metadata", 1, true); - ssMap_ = node.advertiseService("dynamic_map", &SlamToolbox::mapCallback, this); - ssPauseMeasurements_ = node.advertiseService("pause_new_measurements", &SlamToolbox::pauseNewMeasurementsCallback, this); - ssSerialize_ = node.advertiseService("serialize_map", &SlamToolbox::serializePoseGraphCallback, this); - ssDesserialize_ = node.advertiseService("deserialize_map", &SlamToolbox::deserializePoseGraphCallback, this); +/*****************************************************************************/ +{ + tf_ = std::make_unique(ros::Duration(tf_buffer_dur_)); + tfL_ = std::make_unique(*tf_); + tfB_ = std::make_unique(); + sst_ = node.advertise(map_name_, 1, true); + sstm_ = node.advertise(map_name_ + "_metadata", 1, true); + ssMap_ = node.advertiseService("dynamic_map", &SlamToolbox::mapCallback, this); + ssPauseMeasurements_ = node.advertiseService("pause_new_measurements", &SlamToolbox::pauseNewMeasurementsCallback, this); + ssSerialize_ = node.advertiseService("serialize_map", &SlamToolbox::serializePoseGraphCallback, this); + ssDesserialize_ = node.advertiseService("deserialize_map", &SlamToolbox::deserializePoseGraphCallback, this); scan_filter_sub_ = std::make_unique >(node, scan_topic_, 5); scan_filter_ = std::make_unique >(*scan_filter_sub_, *tf_, odom_frame_, 5, node); - scan_filter_->registerCallback(boost::bind(&SlamToolbox::laserCallback, this, _1)); - } + scan_filter_->registerCallback(boost::bind(&SlamToolbox::laserCallback, this, _1)); +} - /*****************************************************************************/ +/*****************************************************************************/ void SlamToolbox::publishTransformLoop(const double& transform_publish_period) - /*****************************************************************************/ - { +/*****************************************************************************/ +{ if(transform_publish_period == 0) - { - return; - } + { + return; + } - ros::Rate r(1.0 / transform_publish_period); + ros::Rate r(1.0 / transform_publish_period); while(ros::ok()) + { { - { - boost::mutex::scoped_lock lock(map_to_odom_mutex_); - geometry_msgs::TransformStamped msg; - tf2::convert(map_to_odom_, msg.transform); - msg.child_frame_id = odom_frame_; - msg.header.frame_id = map_frame_; - msg.header.stamp = ros::Time::now() + transform_timeout_; - tfB_->sendTransform(msg); - } - r.sleep(); + boost::mutex::scoped_lock lock(map_to_odom_mutex_); + geometry_msgs::TransformStamped msg; + tf2::convert(map_to_odom_, msg.transform); + msg.child_frame_id = odom_frame_; + msg.header.frame_id = map_frame_; + msg.header.stamp = ros::Time::now() + transform_timeout_; + tfB_->sendTransform(msg); } + r.sleep(); } +} - /*****************************************************************************/ - void SlamToolbox::publishVisualizations() - /*****************************************************************************/ - { +/*****************************************************************************/ +void SlamToolbox::publishVisualizations() +/*****************************************************************************/ +{ nav_msgs::OccupancyGrid& og = map_.map; - og.info.resolution = resolution_; - og.info.origin.position.x = 0.0; - og.info.origin.position.y = 0.0; - og.info.origin.position.z = 0.0; - og.info.origin.orientation.x = 0.0; - og.info.origin.orientation.y = 0.0; - og.info.origin.orientation.z = 0.0; - og.info.origin.orientation.w = 1.0; - og.header.frame_id = map_frame_; - - double map_update_interval; + og.info.resolution = resolution_; + og.info.origin.position.x = 0.0; + og.info.origin.position.y = 0.0; + og.info.origin.position.z = 0.0; + og.info.origin.orientation.x = 0.0; + og.info.origin.orientation.y = 0.0; + og.info.origin.orientation.z = 0.0; + og.info.origin.orientation.w = 1.0; + og.header.frame_id = map_frame_; + + double map_update_interval; if(!nh_.getParam("map_update_interval", map_update_interval)) - map_update_interval = 10.0; + map_update_interval = 10.0; ros::Rate r(1.0 / map_update_interval); while(ros::ok()) - { - updateMap(); + { + updateMap(); if(!isPaused(VISUALIZING_GRAPH)) { closure_assistant_->publishGraph(); } - r.sleep(); - } + r.sleep(); } +} - /*****************************************************************************/ +/*****************************************************************************/ void SlamToolbox::loadPoseGraphByParams(ros::NodeHandle& nh) - /*****************************************************************************/ +/*****************************************************************************/ +{ + std::string filename; + geometry_msgs::Pose2D pose; + bool dock = false; + if (shouldStartWithPoseGraph(filename, pose, dock)) { - std::string filename; - geometry_msgs::Pose2D pose; - bool dock = false; - if (shouldStartWithPoseGraph(filename, pose, dock)) + slam_toolbox_msgs::DeserializePoseGraph::Request req; + slam_toolbox_msgs::DeserializePoseGraph::Response resp; + req.initial_pose = pose; + req.filename = filename; + if (dock) { - slam_toolbox_msgs::DeserializePoseGraph::Request req; - slam_toolbox_msgs::DeserializePoseGraph::Response resp; - req.initial_pose = pose; - req.filename = filename; - if (dock) - { - req.match_type = - slam_toolbox_msgs::DeserializePoseGraph::Request::START_AT_FIRST_NODE; - } - else - { - req.match_type = - slam_toolbox_msgs::DeserializePoseGraph::Request::START_AT_GIVEN_POSE; - } - deserializePoseGraphCallback(req, resp); + req.match_type = + slam_toolbox_msgs::DeserializePoseGraph::Request::START_AT_FIRST_NODE; } + else + { + req.match_type = + slam_toolbox_msgs::DeserializePoseGraph::Request::START_AT_GIVEN_POSE; + } + deserializePoseGraphCallback(req, resp); } +} - /*****************************************************************************/ +/*****************************************************************************/ bool SlamToolbox::shouldStartWithPoseGraph(std::string& filename, geometry_msgs::Pose2D& pose, bool& start_at_dock) - /*****************************************************************************/ +/*****************************************************************************/ { - // if given a map to load at run time, do it. - if (nh_.getParam("map_file_name", filename)) + // if given a map to load at run time, do it. + if (nh_.getParam("map_file_name", filename)) + { + std::vector read_pose; + if (nh_.getParam("map_start_pose", read_pose)) { - std::vector read_pose; - if (nh_.getParam("map_start_pose", read_pose)) + start_at_dock = false; + if (read_pose.size() != 3) { - start_at_dock = false; - if (read_pose.size() != 3) - { - ROS_ERROR("LocalizationSlamToolbox: Incorrect number of " - "arguments for map starting pose. Must be in format: " - "[x, y, theta]. Starting at the origin"); - pose.x = 0.; - pose.y = 0.; - pose.theta = 0.; - } - else - { - pose.x = read_pose[0]; - pose.y = read_pose[1]; - pose.theta = read_pose[2]; - } + ROS_ERROR("LocalizationSlamToolbox: Incorrect number of " + "arguments for map starting pose. Must be in format: " + "[x, y, theta]. Starting at the origin"); + pose.x = 0.; + pose.y = 0.; + pose.theta = 0.; } else { - nh_.getParam("map_start_at_dock", start_at_dock); + pose.x = read_pose[0]; + pose.y = read_pose[1]; + pose.theta = read_pose[2]; } - - return true; + } + else + { + nh_.getParam("map_start_at_dock", start_at_dock); } - return false; + return true; } - /*****************************************************************************/ + return false; +} + +/*****************************************************************************/ karto::LaserRangeFinder* SlamToolbox::getLaser(const sensor_msgs::LaserScan::ConstPtr& scan) - /*****************************************************************************/ - { +/*****************************************************************************/ +{ const std::string& frame = scan->header.frame_id; if(lasers_.find(frame) == lasers_.end()) + { + try { - try - { - lasers_[frame] = laser_assistant_->toLaserMetadata(*scan); - dataset_->Add(lasers_[frame].getLaser(), true); - } + lasers_[frame] = laser_assistant_->toLaserMetadata(*scan); + dataset_->Add(lasers_[frame].getLaser(), true); + } catch (tf2::TransformException& e) - { - ROS_ERROR("Failed to compute laser pose, aborting initialization (%s)", - e.what()); - return nullptr; - } + { + ROS_ERROR("Failed to compute laser pose, aborting initialization (%s)", + e.what()); + return nullptr; } - - return lasers_[frame].getLaser(); } - /*****************************************************************************/ - bool SlamToolbox::updateMap() - /*****************************************************************************/ + return lasers_[frame].getLaser(); +} + +/*****************************************************************************/ +bool SlamToolbox::updateMap() +/*****************************************************************************/ +{ + if (sst_.getNumSubscribers() == 0) { - if (sst_.getNumSubscribers() == 0) - { - return true; - } - boost::mutex::scoped_lock lock(smapper_mutex_); + return true; + } + boost::mutex::scoped_lock lock(smapper_mutex_); karto::OccupancyGrid* occ_grid = smapper_->getOccupancyGrid(resolution_); if(!occ_grid) - { - return false; - } + { + return false; + } - vis_utils::toNavMap(occ_grid, map_.map); + vis_utils::toNavMap(occ_grid, map_.map); - // publish map as current - map_.map.header.stamp = ros::Time::now(); - sst_.publish(map_.map); - sstm_.publish(map_.map.info); + // publish map as current + map_.map.header.stamp = ros::Time::now(); + sst_.publish(map_.map); + sstm_.publish(map_.map.info); + + delete occ_grid; + occ_grid = nullptr; + return true; +} - delete occ_grid; - occ_grid = nullptr; - return true; - } - - /*****************************************************************************/ - tf2::Stamped SlamToolbox::setTransformFromPoses( +/*****************************************************************************/ +tf2::Stamped SlamToolbox::setTransformFromPoses( const karto::Pose2& corrected_pose, const karto::Pose2& karto_pose, const ros::Time& t, const bool& update_reprocessing_transform) - /*****************************************************************************/ - { - // Compute the map->odom transform - tf2::Stamped odom_to_map; +/*****************************************************************************/ +{ + // Compute the map->odom transform + tf2::Stamped odom_to_map; tf2::Quaternion q(0.,0.,0.,1.0); - q.setRPY(0., 0., corrected_pose.GetHeading()); - tf2::Stamped base_to_map( - tf2::Transform(q, tf2::Vector3(corrected_pose.GetX(), + q.setRPY(0., 0., corrected_pose.GetHeading()); + tf2::Stamped base_to_map( + tf2::Transform(q, tf2::Vector3(corrected_pose.GetX(), corrected_pose.GetY(), 0.0)).inverse(), t, base_frame_); - try - { - geometry_msgs::TransformStamped base_to_map_msg, odom_to_map_msg; - tf2::convert(base_to_map, base_to_map_msg); - odom_to_map_msg = tf_->transform(base_to_map_msg, odom_frame_); - tf2::convert(odom_to_map_msg, odom_to_map); - } + try + { + geometry_msgs::TransformStamped base_to_map_msg, odom_to_map_msg; + tf2::convert(base_to_map, base_to_map_msg); + odom_to_map_msg = tf_->transform(base_to_map_msg, odom_frame_); + tf2::convert(odom_to_map_msg, odom_to_map); + } catch(tf2::TransformException& e) - { - ROS_ERROR("Transform from base_link to odom failed: %s", e.what()); - return odom_to_map; - } + { + ROS_ERROR("Transform from base_link to odom failed: %s", e.what()); + return odom_to_map; + } - // if we're continuing a previous session, we need to - // estimate the homogenous transformation between the old and new - // odometry frames and transform the new session - // into the older session's frame - if (update_reprocessing_transform) - { - tf2::Transform odom_to_base_serialized = base_to_map.inverse(); + // if we're continuing a previous session, we need to + // estimate the homogenous transformation between the old and new + // odometry frames and transform the new session + // into the older session's frame + if (update_reprocessing_transform) + { + tf2::Transform odom_to_base_serialized = base_to_map.inverse(); tf2::Quaternion q1(0.,0.,0.,1.0); - q1.setRPY(0., 0., tf2::getYaw(odom_to_base_serialized.getRotation())); - odom_to_base_serialized.setRotation(q1); - tf2::Transform odom_to_base_current = smapper_->toTfPose(karto_pose); - reprocessing_transform_ = - odom_to_base_serialized * odom_to_base_current.inverse(); - } + q1.setRPY(0., 0., tf2::getYaw(odom_to_base_serialized.getRotation())); + odom_to_base_serialized.setRotation(q1); + tf2::Transform odom_to_base_current = smapper_->toTfPose(karto_pose); + reprocessing_transform_ = + odom_to_base_serialized * odom_to_base_current.inverse(); + } - // set map to odom for our transformation thread to publish - boost::mutex::scoped_lock lock(map_to_odom_mutex_); + // set map to odom for our transformation thread to publish + boost::mutex::scoped_lock lock(map_to_odom_mutex_); map_to_odom_ = tf2::Transform(tf2::Quaternion( odom_to_map.getRotation() ), tf2::Vector3( odom_to_map.getOrigin() ) ).inverse(); - return odom_to_map; - } + return odom_to_map; +} - /*****************************************************************************/ +/*****************************************************************************/ karto::LocalizedRangeScan* SlamToolbox::getLocalizedRangeScan( karto::LaserRangeFinder* laser, const sensor_msgs::LaserScan::ConstPtr& scan, karto::Pose2& karto_pose) - /*****************************************************************************/ - { - // Create a vector of doubles for karto - std::vector readings = laser_utils::scanToReadings( - *scan, lasers_[scan->header.frame_id].isInverted()); +/*****************************************************************************/ +{ + // Create a vector of doubles for karto + std::vector readings = laser_utils::scanToReadings( + *scan, lasers_[scan->header.frame_id].isInverted()); - // transform by the reprocessing transform - tf2::Transform pose_original = smapper_->toTfPose(karto_pose); - tf2::Transform tf_pose_transformed = reprocessing_transform_ * pose_original; - karto::Pose2 transformed_pose = smapper_->toKartoPose(tf_pose_transformed); + // transform by the reprocessing transform + tf2::Transform pose_original = smapper_->toTfPose(karto_pose); + tf2::Transform tf_pose_transformed = reprocessing_transform_ * pose_original; + karto::Pose2 transformed_pose = smapper_->toKartoPose(tf_pose_transformed); - // create localized range scan + // create localized range scan karto::LocalizedRangeScan* range_scan = new karto::LocalizedRangeScan( - laser->GetName(), readings); - range_scan->SetOdometricPose(transformed_pose); - range_scan->SetCorrectedPose(transformed_pose); - return range_scan; - } - - /*****************************************************************************/ - bool SlamToolbox::shouldProcessScan( + laser->GetName(), readings); + range_scan->SetOdometricPose(transformed_pose); + range_scan->SetCorrectedPose(transformed_pose); + return range_scan; +} + +/*****************************************************************************/ +bool SlamToolbox::shouldProcessScan( const sensor_msgs::LaserScan::ConstPtr& scan, const karto::Pose2& pose) - /*****************************************************************************/ - { - static karto::Pose2 last_pose; - static ros::Time last_scan_time = ros::Time(0.); +/*****************************************************************************/ +{ + static karto::Pose2 last_pose; + static ros::Time last_scan_time = ros::Time(0.); static const double dist_thresh_sq = smapper_->getMapper()->getParamMinimumTravelDistance()* - smapper_->getMapper()->getParamMinimumTravelDistance(); + smapper_->getMapper()->getParamMinimumTravelDistance(); - // we give it a pass on the first measurement to get the ball rolling - if (first_measurement_) - { - last_scan_time = scan->header.stamp; - last_pose = pose; - first_measurement_ = false; - return true; - } + // we give it a pass on the first measurement to get the ball rolling + if (first_measurement_) + { + last_scan_time = scan->header.stamp; + last_pose = pose; + first_measurement_ = false; + return true; + } - // we are in a paused mode, reject incomming information + // we are in a paused mode, reject incomming information if(isPaused(NEW_MEASUREMENTS)) - { - return false; - } + { + return false; + } - // throttled out - if ((scan->header.seq % throttle_scans_) != 0) - { - return false; - } + // throttled out + if ((scan->header.seq % throttle_scans_) != 0) + { + return false; + } - // not enough time - if (scan->header.stamp - last_scan_time < minimum_time_interval_) - { - return false; - } + // not enough time + if (scan->header.stamp - last_scan_time < minimum_time_interval_) + { + return false; + } - // check moved enough, within 10% for correction error - const double sq_dist_to_last_accepted_pose = last_pose.SquaredDistance(pose); + // check moved enough, within 10% for correction error + const double sq_dist_to_last_accepted_pose = last_pose.SquaredDistance(pose); if(sq_dist_to_last_accepted_pose < 0.8 * dist_thresh_sq || scan->header.seq < 5) - { - return false; - } + { + return false; + } - last_pose = pose; - last_scan_time = scan->header.stamp; + last_pose = pose; + last_scan_time = scan->header.stamp; - return true; - } + return true; +} - /*****************************************************************************/ +/*****************************************************************************/ karto::LocalizedRangeScan* SlamToolbox::addScan( karto::LaserRangeFinder* laser, PosedScan& scan_w_pose) - /*****************************************************************************/ - { - return addScan(laser, scan_w_pose.scan, scan_w_pose.pose); - } +/*****************************************************************************/ +{ + return addScan(laser, scan_w_pose.scan, scan_w_pose.pose); +} - /*****************************************************************************/ +/*****************************************************************************/ karto::LocalizedRangeScan* SlamToolbox::addScan( karto::LaserRangeFinder* laser, const sensor_msgs::LaserScan::ConstPtr& scan, karto::Pose2& karto_pose) - /*****************************************************************************/ - { - // get our localized range scan +/*****************************************************************************/ +{ + // get our localized range scan karto::LocalizedRangeScan* range_scan = getLocalizedRangeScan( - laser, scan, karto_pose); + laser, scan, karto_pose); - // Add the localized range scan to the smapper - boost::mutex::scoped_lock lock(smapper_mutex_); - bool processed = false, update_reprocessing_transform = false; + // Add the localized range scan to the smapper + boost::mutex::scoped_lock lock(smapper_mutex_); + bool processed = false, update_reprocessing_transform = false; - if (processor_type_ == PROCESS) - { - processed = smapper_->getMapper()->Process(range_scan); - } - else if (processor_type_ == PROCESS_FIRST_NODE) - { - processed = smapper_->getMapper()->ProcessAtDock(range_scan); - 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_) - { - ROS_ERROR("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); - update_reprocessing_transform = true; - processor_type_ = PROCESS; - } - else - { - ROS_FATAL("SlamToolbox: No valid processor type set! Exiting."); - exit(-1); - } + if (processor_type_ == PROCESS) + { + processed = smapper_->getMapper()->Process(range_scan); + } + else if (processor_type_ == PROCESS_FIRST_NODE) + { + processed = smapper_->getMapper()->ProcessAtDock(range_scan); + 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_) + { + ROS_ERROR("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); + update_reprocessing_transform = true; + processor_type_ = PROCESS; + } + else + { + ROS_FATAL("SlamToolbox: No valid processor type set! Exiting."); + exit(-1); + } - // if successfully processed, create odom to map transformation - // and add our scan to storage + // if successfully processed, create odom to map transformation + // and add our scan to storage if(processed) + { + if (enable_interactive_mode_) { - if (enable_interactive_mode_) - { - scan_holder_->addScan(*scan); - } - - setTransformFromPoses(range_scan->GetCorrectedPose(), karto_pose, - scan->header.stamp, update_reprocessing_transform); - dataset_->Add(range_scan); - } - else - { - delete range_scan; - range_scan = nullptr; + scan_holder_->addScan(*scan); } - return range_scan; + setTransformFromPoses(range_scan->GetCorrectedPose(), karto_pose, + scan->header.stamp, update_reprocessing_transform); + dataset_->Add(range_scan); } - - /*****************************************************************************/ - bool SlamToolbox::mapCallback( - nav_msgs::GetMap::Request &req, - nav_msgs::GetMap::Response &res) - /*****************************************************************************/ + else { + delete range_scan; + range_scan = nullptr; + } + + return range_scan; +} + +/*****************************************************************************/ +bool SlamToolbox::mapCallback( + nav_msgs::GetMap::Request &req, + nav_msgs::GetMap::Response &res) +/*****************************************************************************/ +{ if(map_.map.info.width && map_.map.info.height) - { - boost::mutex::scoped_lock lock(smapper_mutex_); - res = map_; - return true; - } - else - { - return false; - } + { + boost::mutex::scoped_lock lock(smapper_mutex_); + res = map_; + return true; } + else + { + return false; + } +} - /*****************************************************************************/ - bool SlamToolbox::pauseNewMeasurementsCallback( +/*****************************************************************************/ +bool SlamToolbox::pauseNewMeasurementsCallback( slam_toolbox_msgs::Pause::Request& req, slam_toolbox_msgs::Pause::Response& resp) - /*****************************************************************************/ - { - bool curr_state = isPaused(NEW_MEASUREMENTS); - state_.set(NEW_MEASUREMENTS, !curr_state); +/*****************************************************************************/ +{ + bool curr_state = isPaused(NEW_MEASUREMENTS); + state_.set(NEW_MEASUREMENTS, !curr_state); - nh_.setParam("paused_new_measurements", !curr_state); - ROS_INFO("SlamToolbox: Toggled to %s", + nh_.setParam("paused_new_measurements", !curr_state); + ROS_INFO("SlamToolbox: Toggled to %s", !curr_state ? "pause taking new measurements." : "actively taking new measurements."); - resp.status = true; - return true; - } + resp.status = true; + return true; +} - /*****************************************************************************/ +/*****************************************************************************/ bool SlamToolbox::isPaused(const PausedApplication& app) - /*****************************************************************************/ - { - return state_.get(app); - } +/*****************************************************************************/ +{ + return state_.get(app); +} - /*****************************************************************************/ - bool SlamToolbox::serializePoseGraphCallback( +/*****************************************************************************/ +bool SlamToolbox::serializePoseGraphCallback( slam_toolbox_msgs::SerializePoseGraph::Request &req, - slam_toolbox_msgs::SerializePoseGraph::Response &resp) - /*****************************************************************************/ - { - std::string filename = req.filename; - - // if we're inside the snap, we need to write to commonly accessible space - if (snap_utils::isInSnap()) - { - filename = snap_utils::getSnapPath() + std::string("/") + filename; - } + slam_toolbox_msgs::SerializePoseGraph::Response &resp) +/*****************************************************************************/ +{ + std::string filename = req.filename; - boost::mutex::scoped_lock lock(smapper_mutex_); - serialization::write(filename, *smapper_->getMapper(), *dataset_); - return true; + // if we're inside the snap, we need to write to commonly accessible space + if (snap_utils::isInSnap()) + { + filename = snap_utils::getSnapPath() + std::string("/") + filename; } - /*****************************************************************************/ - void SlamToolbox::loadSerializedPoseGraph( + boost::mutex::scoped_lock lock(smapper_mutex_); + serialization::write(filename, *smapper_->getMapper(), *dataset_); + return true; +} + +/*****************************************************************************/ +void SlamToolbox::loadSerializedPoseGraph( std::unique_ptr& mapper, std::unique_ptr& dataset) - /*****************************************************************************/ - { - boost::mutex::scoped_lock lock(smapper_mutex_); +/*****************************************************************************/ +{ + boost::mutex::scoped_lock lock(smapper_mutex_); - solver_->Reset(); + solver_->Reset(); - // add the nodes and constraints to the optimizer - VerticeMap mapper_vertices = mapper->GetGraph()->GetVertices(); - VerticeMap::iterator vertex_map_it = mapper_vertices.begin(); + // add the nodes and constraints to the optimizer + VerticeMap mapper_vertices = mapper->GetGraph()->GetVertices(); + VerticeMap::iterator vertex_map_it = mapper_vertices.begin(); for(vertex_map_it; vertex_map_it != mapper_vertices.end(); ++vertex_map_it) - { - ScanMap::iterator vertex_it = vertex_map_it->second.begin(); + { + ScanMap::iterator vertex_it = vertex_map_it->second.begin(); for(vertex_it; vertex_it != vertex_map_it->second.end(); ++vertex_it) + { + if (vertex_it->second != nullptr) { - if (vertex_it->second != nullptr) - { - solver_->AddNode(vertex_it->second); - } + solver_->AddNode(vertex_it->second); } } + } - EdgeVector mapper_edges = mapper->GetGraph()->GetEdges(); - EdgeVector::iterator edges_it = mapper_edges.begin(); + EdgeVector mapper_edges = mapper->GetGraph()->GetEdges(); + EdgeVector::iterator edges_it = mapper_edges.begin(); for( edges_it; edges_it != mapper_edges.end(); ++edges_it) + { + if (*edges_it != nullptr) { - if (*edges_it != nullptr) - { - solver_->AddConstraint(*edges_it); - } + solver_->AddConstraint(*edges_it); } + } - mapper->SetScanSolver(solver_.get()); + mapper->SetScanSolver(solver_.get()); - // move the memory to our working dataset - smapper_->setMapper(mapper.release()); - smapper_->configure(nh_); - dataset_.reset(dataset.release()); - closure_assistant_->setMapper(smapper_->getMapper()); + // move the memory to our working dataset + smapper_->setMapper(mapper.release()); + smapper_->configure(nh_); + dataset_.reset(dataset.release()); + closure_assistant_->setMapper(smapper_->getMapper()); - if (!smapper_->getMapper()) - { - ROS_FATAL("loadSerializedPoseGraph: Could not properly load " - "a valid mapping object. Did you modify something by hand?"); - exit(-1); - } + if (!smapper_->getMapper()) + { + ROS_FATAL("loadSerializedPoseGraph: Could not properly load " + "a valid mapping object. Did you modify something by hand?"); + exit(-1); + } - if (dataset_->GetLasers().size() < 1) - { - ROS_FATAL("loadSerializedPoseGraph: Cannot deserialize " - "dataset with no laser objects."); - exit(-1); - } + if (dataset_->GetLasers().size() < 1) + { + ROS_FATAL("loadSerializedPoseGraph: Cannot deserialize " + "dataset with no laser objects."); + exit(-1); + } - // create a current laser sensor + // create a current laser sensor karto::LaserRangeFinder* laser = dynamic_cast( - dataset_->GetLasers()[0]); + dataset_->GetLasers()[0]); karto::Sensor* pSensor = dynamic_cast(laser); - if (pSensor) - { - karto::SensorManager::GetInstance()->RegisterSensor(pSensor); + if (pSensor) + { + karto::SensorManager::GetInstance()->RegisterSensor(pSensor); - while (ros::ok()) + while (ros::ok()) + { + ROS_INFO("Waiting for incoming scan to get metadata..."); + boost::shared_ptr scan = + ros::topic::waitForMessage( + scan_topic_, ros::Duration(1.0)); + if (scan) { - ROS_INFO("Waiting for incoming scan to get metadata..."); - boost::shared_ptr scan = - ros::topic::waitForMessage( - scan_topic_, ros::Duration(1.0)); - if (scan) + ROS_INFO("Got scan!"); + try { - ROS_INFO("Got scan!"); - try - { - lasers_[scan->header.frame_id] = - laser_assistant_->toLaserMetadata(*scan); - break; - } + lasers_[scan->header.frame_id] = + laser_assistant_->toLaserMetadata(*scan); + break; + } catch (tf2::TransformException& e) - { - ROS_ERROR("Failed to compute laser pose, aborting continue mapping (%s)", - e.what()); - exit(-1); - } + { + ROS_ERROR("Failed to compute laser pose, aborting continue mapping (%s)", + e.what()); + exit(-1); } } } - else - { - ROS_ERROR("Invalid sensor pointer in dataset. Unable to register sensor."); - } + } + else + { + ROS_ERROR("Invalid sensor pointer in dataset. Unable to register sensor."); + } - solver_->Compute(); + solver_->Compute(); - return; - } + return; +} - /*****************************************************************************/ - bool SlamToolbox::deserializePoseGraphCallback( +/*****************************************************************************/ +bool SlamToolbox::deserializePoseGraphCallback( slam_toolbox_msgs::DeserializePoseGraph::Request &req, - slam_toolbox_msgs::DeserializePoseGraph::Response &resp) - /*****************************************************************************/ + slam_toolbox_msgs::DeserializePoseGraph::Response &resp) +/*****************************************************************************/ +{ + if (req.match_type == slam_toolbox_msgs::DeserializePoseGraph::Request::UNSET) { - if (req.match_type == slam_toolbox_msgs::DeserializePoseGraph::Request::UNSET) - { - ROS_ERROR("Deserialization called without valid processor type set. " - "Undefined behavior!"); - return false; - } + ROS_ERROR("Deserialization called without valid processor type set. " + "Undefined behavior!"); + return false; + } - std::string filename = req.filename; + std::string filename = req.filename; - if (filename.empty()) - { - ROS_WARN("No map file given!"); - return true; - } + if (filename.empty()) + { + ROS_WARN("No map file given!"); + return true; + } - // if we're inside the snap, we need to write to commonly accessible space - if (snap_utils::isInSnap()) - { - filename = snap_utils::getSnapPath() + std::string("/") + filename; - } + // if we're inside the snap, we need to write to commonly accessible space + if (snap_utils::isInSnap()) + { + filename = snap_utils::getSnapPath() + std::string("/") + filename; + } - std::unique_ptr dataset = std::make_unique(); - std::unique_ptr mapper = std::make_unique(); + std::unique_ptr dataset = std::make_unique(); + std::unique_ptr mapper = std::make_unique(); - if (!serialization::read(filename, *mapper, *dataset)) - { - ROS_ERROR("DeserializePoseGraph: Failed to read " + if (!serialization::read(filename, *mapper, *dataset)) + { + ROS_ERROR("DeserializePoseGraph: Failed to read " "file: %s.", filename.c_str()); - return true; - } - ROS_DEBUG("DeserializePoseGraph: Successfully read file."); + return true; + } + ROS_DEBUG("DeserializePoseGraph: Successfully read file."); - loadSerializedPoseGraph(mapper, dataset); - updateMap(); + loadSerializedPoseGraph(mapper, dataset); + updateMap(); - first_measurement_ = true; - boost::mutex::scoped_lock l(pose_mutex_); - switch (req.match_type) - { + first_measurement_ = true; + boost::mutex::scoped_lock l(pose_mutex_); + switch (req.match_type) + { case procType::START_AT_FIRST_NODE: processor_type_ = PROCESS_FIRST_NODE; break; case procType::START_AT_GIVEN_POSE: processor_type_ = PROCESS_NEAR_REGION; - process_near_pose_ = std::make_unique(req.initial_pose.x, - req.initial_pose.y, req.initial_pose.theta); + process_near_pose_ = std::make_unique(req.initial_pose.x, + req.initial_pose.y, req.initial_pose.theta); break; - case procType::LOCALIZE_AT_POSE: + case procType::LOCALIZE_AT_POSE: processor_type_ = PROCESS_LOCALIZATION; - process_near_pose_ = std::make_unique(req.initial_pose.x, - req.initial_pose.y, req.initial_pose.theta); + process_near_pose_ = std::make_unique(req.initial_pose.x, + req.initial_pose.y, req.initial_pose.theta); break; default: ROS_FATAL("Deserialization called without valid processor type set."); - } - - return true; } + return true; +} + } // end namespace From 3b0760870bb6fa7ea2187f7233bc508459bd0098 Mon Sep 17 00:00:00 2001 From: suryakanth Date: Sat, 12 Nov 2022 00:28:50 +0530 Subject: [PATCH 05/13] Reverted all unnecessary code reformatting --- slam_toolbox/src/loop_closure_assistant.cpp | 22 ++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/slam_toolbox/src/loop_closure_assistant.cpp b/slam_toolbox/src/loop_closure_assistant.cpp index 81a10ffc6..0f59cf87f 100644 --- a/slam_toolbox/src/loop_closure_assistant.cpp +++ b/slam_toolbox/src/loop_closure_assistant.cpp @@ -157,20 +157,20 @@ visualization_msgs::Marker LoopClosureAssistant::getMarker(std::string name, int void LoopClosureAssistant::publishGraph() /*****************************************************************************/ { - interactive_server_->clear(); + interactive_server_->clear(); std::unordered_map* graph = solver_->getGraph(); - if (graph->size() == 0) - { - return; - } + if (graph->size() == 0) + { + return; + } ROS_DEBUG("Graph size: %i",(int)graph->size()); - bool interactive_mode = false; - { - boost::mutex::scoped_lock lock(interactive_mutex_); - interactive_mode = interactive_mode_; - } + bool interactive_mode = false; + { + boost::mutex::scoped_lock lock(interactive_mutex_); + interactive_mode = interactive_mode_; + } if (!mapper_->GetGraph()) ROS_ERROR_STREAM("Invalid pointer"); @@ -225,7 +225,7 @@ void LoopClosureAssistant::publishGraph() vis_utils::toInteractiveMarker(m, 0.3); interactive_server_->insert(int_marker, boost::bind( - &LoopClosureAssistant::processInteractiveFeedback, + &LoopClosureAssistant::processInteractiveFeedback, this, _1)); } else From 39fae42850b2e2c8b2fcdc69aab05a9bdacb778e Mon Sep 17 00:00:00 2001 From: suryakanth Date: Sat, 12 Nov 2022 00:30:09 +0530 Subject: [PATCH 06/13] Reverted all unnecessary code reformatting --- slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp | 1 - slam_toolbox/src/loop_closure_assistant.cpp | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp b/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp index a47ddd8c3..4f1e10aac 100644 --- a/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp +++ b/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp @@ -73,7 +73,6 @@ class LoopClosureAssistant std::string map_frame_; PausedState& state_; ProcessType& processor_type_; - bool isInter = false; }; } // end namespace diff --git a/slam_toolbox/src/loop_closure_assistant.cpp b/slam_toolbox/src/loop_closure_assistant.cpp index 0f59cf87f..ba5cef896 100644 --- a/slam_toolbox/src/loop_closure_assistant.cpp +++ b/slam_toolbox/src/loop_closure_assistant.cpp @@ -244,7 +244,7 @@ void LoopClosureAssistant::publishGraph() geometry_msgs::Point prevSourcePoint, prevTargetPoint; for (const auto &edge : edges) { - isInter = false; + bool isInter = false; std_msgs::ColorRGBA color; From f29c7ccd54df1a4c66e567e8936c6826ca50d091 Mon Sep 17 00:00:00 2001 From: suryakanth Date: Sat, 12 Nov 2022 00:32:16 +0530 Subject: [PATCH 07/13] auto formating updated --- slam_toolbox/src/loop_closure_assistant.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/slam_toolbox/src/loop_closure_assistant.cpp b/slam_toolbox/src/loop_closure_assistant.cpp index ba5cef896..d1dddb5f3 100644 --- a/slam_toolbox/src/loop_closure_assistant.cpp +++ b/slam_toolbox/src/loop_closure_assistant.cpp @@ -168,7 +168,7 @@ void LoopClosureAssistant::publishGraph() ROS_DEBUG("Graph size: %i",(int)graph->size()); bool interactive_mode = false; { - boost::mutex::scoped_lock lock(interactive_mutex_); + boost::mutex::scoped_lock lock(interactive_mutex_); interactive_mode = interactive_mode_; } From 92c8eae72162d93b6f39fa9fd3cc263a0082e2c4 Mon Sep 17 00:00:00 2001 From: suryakanth Date: Sat, 12 Nov 2022 01:20:53 +0530 Subject: [PATCH 08/13] auto formating updated --- slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp b/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp index 4f1e10aac..b7cb8ad1a 100644 --- a/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp +++ b/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp @@ -48,6 +48,7 @@ class LoopClosureAssistant void processInteractiveFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); void publishGraph(); void setMapper(karto::Mapper * mapper); + private: bool manualLoopClosureCallback(slam_toolbox_msgs::LoopClosure::Request& req, slam_toolbox_msgs::LoopClosure::Response& resp); bool clearChangesCallback(slam_toolbox_msgs::Clear::Request& req, slam_toolbox_msgs::Clear::Response& resp); From 6287ba670b3743a9c5fc18393d8fa7ffa0651960 Mon Sep 17 00:00:00 2001 From: suryakanth Date: Sat, 12 Nov 2022 01:25:24 +0530 Subject: [PATCH 09/13] diff fix and comments added --- .../include/slam_toolbox/loop_closure_assistant.hpp | 3 +-- slam_toolbox/src/loop_closure_assistant.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp b/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp index b7cb8ad1a..6794163ad 100644 --- a/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp +++ b/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp @@ -48,14 +48,13 @@ class LoopClosureAssistant void processInteractiveFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); void publishGraph(); void setMapper(karto::Mapper * mapper); - + private: bool manualLoopClosureCallback(slam_toolbox_msgs::LoopClosure::Request& req, slam_toolbox_msgs::LoopClosure::Response& resp); bool clearChangesCallback(slam_toolbox_msgs::Clear::Request& req, slam_toolbox_msgs::Clear::Response& resp); bool interactiveModeCallback(slam_toolbox_msgs::ToggleInteractive::Request &req, slam_toolbox_msgs::ToggleInteractive::Response &resp); void moveNode(const int& id, const Eigen::Vector3d& pose); void addMovedNodes(const int& id, Eigen::Vector3d vec); - std_msgs::ColorRGBA getColor(float r, float g, float b); visualization_msgs::Marker getMarker( std::string name,int reserve); diff --git a/slam_toolbox/src/loop_closure_assistant.cpp b/slam_toolbox/src/loop_closure_assistant.cpp index d1dddb5f3..ad6bfa0c7 100644 --- a/slam_toolbox/src/loop_closure_assistant.cpp +++ b/slam_toolbox/src/loop_closure_assistant.cpp @@ -196,7 +196,7 @@ void LoopClosureAssistant::publishGraph() marray.markers.push_back(clear); visualization_msgs::Marker slam_node = vis_utils::toMarker(map_frame_, "slam_nodes", 0.05); - visualization_msgs::Marker loc_node = vis_utils::toMarker(map_frame_, "loc_node", 0.05); + visualization_msgs::Marker loc_node = vis_utils::toMarker(map_frame_, "loc_nodes", 0.05); // add map nodes for (const auto &sensor_name : vertices) @@ -242,6 +242,7 @@ void LoopClosureAssistant::publishGraph() visualization_msgs::Marker inter_loc_edges_marker = getMarker("inter_loc_edges", localization_vertices.size() * 3); geometry_msgs::Point prevSourcePoint, prevTargetPoint; + for (const auto &edge : edges) { bool isInter = false; @@ -255,21 +256,20 @@ void LoopClosureAssistant::publishGraph() geometry_msgs::Point sourcePoint; sourcePoint.x = pose0.GetX(); sourcePoint.y = pose0.GetY(); - // if (prevTargetPoint == sourcePoint) int target_id = edge->GetTarget()->GetObject()->GetUniqueId(); const auto &pose1 = edge->GetTarget()->GetObject()->GetCorrectedPose(); geometry_msgs::Point targetPoint; targetPoint.x = pose1.GetX(); targetPoint.y = pose1.GetY(); - if (prevTargetPoint == targetPoint) + if (prevTargetPoint == targetPoint) //checking for inter constraints { isInter = true; color = (source_id >= first_localization_id) ? getColor(1, 0, 1) : getColor(0, 1, 1); } else { - color = getColor(1, 1, 0); // yello + color = getColor(1, 1, 0); } prevSourcePoint = sourcePoint; @@ -279,7 +279,7 @@ void LoopClosureAssistant::publishGraph() { if (isInter) { - inter_loc_edges_marker.colors.push_back(color); + inter_loc_edges_marker.colors.push_back(color); inter_loc_edges_marker.colors.push_back(color); inter_loc_edges_marker.points.push_back(sourcePoint); inter_loc_edges_marker.points.push_back(targetPoint); From 7ec8f2f87002a79b4f4f0efa568ac86a6506301d Mon Sep 17 00:00:00 2001 From: suryakanth Date: Sat, 12 Nov 2022 01:27:34 +0530 Subject: [PATCH 10/13] all diff fixed --- slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h | 1 + 1 file changed, 1 insertion(+) diff --git a/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h b/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h index 90a068540..e732e468d 100644 --- a/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -2056,6 +2056,7 @@ namespace karto { return m_pMapperSensorManager; } + /** * Gets the localization vertices * @return localization vertices From f7d0b5d18b1f3de368b89883176b51c42d38ab91 Mon Sep 17 00:00:00 2001 From: suryakanth Date: Sat, 12 Nov 2022 02:12:45 +0530 Subject: [PATCH 11/13] visualization functions moved to visualization_utils --- .../slam_toolbox/loop_closure_assistant.hpp | 2 - .../slam_toolbox/visualization_utils.hpp | 35 +++++++++++++++++ slam_toolbox/src/loop_closure_assistant.cpp | 39 +++---------------- 3 files changed, 41 insertions(+), 35 deletions(-) diff --git a/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp b/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp index 6794163ad..4c408970e 100644 --- a/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp +++ b/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp @@ -55,8 +55,6 @@ class LoopClosureAssistant bool interactiveModeCallback(slam_toolbox_msgs::ToggleInteractive::Request &req, slam_toolbox_msgs::ToggleInteractive::Response &resp); void moveNode(const int& id, const Eigen::Vector3d& pose); void addMovedNodes(const int& id, Eigen::Vector3d vec); - std_msgs::ColorRGBA getColor(float r, float g, float b); - visualization_msgs::Marker getMarker( std::string name,int reserve); std::unique_ptr tfB_; laser_utils::ScanHolder* scan_holder_; diff --git a/slam_toolbox/include/slam_toolbox/visualization_utils.hpp b/slam_toolbox/include/slam_toolbox/visualization_utils.hpp index 3a42a85fa..a41ae7056 100644 --- a/slam_toolbox/include/slam_toolbox/visualization_utils.hpp +++ b/slam_toolbox/include/slam_toolbox/visualization_utils.hpp @@ -139,6 +139,41 @@ inline void toNavMap( return; } +inline std_msgs::ColorRGBA getColor( + const float& r, + const float& g, + const float& b) +{ + std_msgs::ColorRGBA color; + color.r = r; + color.g = g; + color.b = b; + return color; +} + +inline visualization_msgs::Marker getMarker( + const std::string& frame, + const std::string& name, + const int& reserve) +{ + visualization_msgs::Marker edges_marker; + edges_marker.header.frame_id = frame; + edges_marker.header.stamp = ros::Time::now(); + edges_marker.id = 0; + edges_marker.ns = name; + edges_marker.action = visualization_msgs::Marker::ADD; + edges_marker.type = visualization_msgs::Marker::LINE_LIST; + edges_marker.pose.orientation.w = 1; + edges_marker.scale.x = 0.01; + edges_marker.color.r = 0.0; + edges_marker.color.g = 0.0; + edges_marker.color.b = 0.0; + edges_marker.color.a = 1; + edges_marker.lifetime = ros::Duration(0.0); + edges_marker.points.reserve(reserve); + return edges_marker; +} + } // end namespace #endif //SLAM_TOOLBOX_VISUALIZATION_UTILS_H_ diff --git a/slam_toolbox/src/loop_closure_assistant.cpp b/slam_toolbox/src/loop_closure_assistant.cpp index ad6bfa0c7..96393fff0 100644 --- a/slam_toolbox/src/loop_closure_assistant.cpp +++ b/slam_toolbox/src/loop_closure_assistant.cpp @@ -126,33 +126,6 @@ void LoopClosureAssistant::setMapper(karto::Mapper *mapper) mapper_ = mapper; } -std_msgs::ColorRGBA LoopClosureAssistant::getColor(float r, float g, float b) -{ - std_msgs::ColorRGBA color; - color.r = r; - color.g = g; - color.b = b; - return color; -} -visualization_msgs::Marker LoopClosureAssistant::getMarker(std::string name, int reserve) -{ - visualization_msgs::Marker edges_marker; - edges_marker.header.frame_id = map_frame_; - edges_marker.header.stamp = ros::Time::now(); - edges_marker.id = 0; - edges_marker.ns = name; - edges_marker.action = visualization_msgs::Marker::ADD; - edges_marker.type = visualization_msgs::Marker::LINE_LIST; - edges_marker.pose.orientation.w = 1; - edges_marker.scale.x = 0.01; - edges_marker.color.r = 0.0; - edges_marker.color.g = 0.0; - edges_marker.color.b = 0.0; - edges_marker.color.a = 1; - edges_marker.lifetime = ros::Duration(0.0); - edges_marker.points.reserve(reserve); - return edges_marker; -} /*****************************************************************************/ void LoopClosureAssistant::publishGraph() /*****************************************************************************/ @@ -236,10 +209,10 @@ void LoopClosureAssistant::publishGraph() } // add line markers for graph edges - visualization_msgs::Marker edges_marker = getMarker("intra_slam_edges", edges.size() * 2); - visualization_msgs::Marker edges_marker_inter = getMarker("inter_slam_edges", edges.size() * 2); - visualization_msgs::Marker loc_edges_marker = getMarker("intra_loc_edges", localization_vertices.size() * 3); - visualization_msgs::Marker inter_loc_edges_marker = getMarker("inter_loc_edges", localization_vertices.size() * 3); + visualization_msgs::Marker edges_marker = vis_utils::getMarker(map_frame_,"intra_slam_edges", edges.size() * 2); + visualization_msgs::Marker edges_marker_inter = vis_utils::getMarker(map_frame_,"inter_slam_edges", edges.size() * 2); + visualization_msgs::Marker loc_edges_marker = vis_utils::getMarker(map_frame_,"intra_loc_edges", localization_vertices.size() * 3); + visualization_msgs::Marker inter_loc_edges_marker = vis_utils::getMarker(map_frame_,"inter_loc_edges", localization_vertices.size() * 3); geometry_msgs::Point prevSourcePoint, prevTargetPoint; @@ -265,11 +238,11 @@ void LoopClosureAssistant::publishGraph() if (prevTargetPoint == targetPoint) //checking for inter constraints { isInter = true; - color = (source_id >= first_localization_id) ? getColor(1, 0, 1) : getColor(0, 1, 1); + color = (source_id >= first_localization_id) ? vis_utils::getColor(1, 0, 1) : vis_utils::getColor(0, 1, 1); } else { - color = getColor(1, 1, 0); + color = vis_utils::getColor(1, 1, 0); } prevSourcePoint = sourcePoint; From e380943003b0c5fde35f5887c4cccf7030544d7d Mon Sep 17 00:00:00 2001 From: suryakanth Date: Mon, 14 Nov 2022 16:07:20 +0530 Subject: [PATCH 12/13] interactive marker out of range crash fix --- slam_toolbox/src/loop_closure_assistant.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/slam_toolbox/src/loop_closure_assistant.cpp b/slam_toolbox/src/loop_closure_assistant.cpp index 96393fff0..de9951413 100644 --- a/slam_toolbox/src/loop_closure_assistant.cpp +++ b/slam_toolbox/src/loop_closure_assistant.cpp @@ -188,7 +188,7 @@ void LoopClosureAssistant::publishGraph() } const auto &pose = vertex.second->GetObject()->GetCorrectedPose(); - m.id = vertex.first; + m.id = vertex.first + 1; m.pose.position.x = pose.GetX(); m.pose.position.y = pose.GetY(); From 4eac0a1f044f7190ac614469761333139c0a1db3 Mon Sep 17 00:00:00 2001 From: suryakanth Date: Mon, 14 Nov 2022 16:25:06 +0530 Subject: [PATCH 13/13] marker scale reset --- slam_toolbox/src/loop_closure_assistant.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/slam_toolbox/src/loop_closure_assistant.cpp b/slam_toolbox/src/loop_closure_assistant.cpp index de9951413..82589bf8f 100644 --- a/slam_toolbox/src/loop_closure_assistant.cpp +++ b/slam_toolbox/src/loop_closure_assistant.cpp @@ -168,8 +168,8 @@ void LoopClosureAssistant::publishGraph() clear.action = visualization_msgs::Marker::DELETEALL; marray.markers.push_back(clear); - visualization_msgs::Marker slam_node = vis_utils::toMarker(map_frame_, "slam_nodes", 0.05); - visualization_msgs::Marker loc_node = vis_utils::toMarker(map_frame_, "loc_nodes", 0.05); + visualization_msgs::Marker slam_node = vis_utils::toMarker(map_frame_, "slam_nodes", 0.1); + visualization_msgs::Marker loc_node = vis_utils::toMarker(map_frame_, "loc_nodes", 0.1); // add map nodes for (const auto &sensor_name : vertices)