29 : solver_loader_(
"slam_toolbox",
"karto::ScanSolver"),
31 first_measurement_(true),
33 process_near_pose_(nullptr)
36 smapper_ = std::make_unique<mapper_utils::SMapper>();
37 dataset_ = std::make_unique<karto::Dataset>();
45 pose_helper_ = std::make_unique<pose_utils::GetPoseHelper>(
50 std::make_unique<loop_closure_assistant::LoopClosureAssistant>(
55 double transform_publish_period;
56 nh_.
param(
"transform_publish_period", transform_publish_period, 0.05);
57 threads_.push_back(std::make_unique<boost::thread>(
59 this, transform_publish_period)));
60 threads_.push_back(std::make_unique<boost::thread>(
68 for (
int i=0; i !=
threads_.size(); i++)
87 std::string solver_plugin;
88 if(!private_nh_.
getParam(
"solver_plugin", solver_plugin))
90 ROS_WARN(
"unable to find requested solver plugin, defaulting to SPA");
91 solver_plugin =
"solver_plugins::CeresSolver";
96 ROS_INFO(
"Using plugin %s", solver_plugin.c_str());
100 ROS_FATAL(
"Failed to create %s, is it registered and built? Exception: %s.",
101 solver_plugin.c_str(), ex.what());
122 private_nh.
param(
"transform_timeout", tmp_val, 0.2);
124 private_nh.
param(
"tf_buffer_duration", tmp_val, 30.);
126 private_nh.
param(
"minimum_time_interval", tmp_val, 0.5);
133 if (private_nh.
getParam(
"debug_logging", debug) && debug)
143 private_nh.
setParam(
"paused_new_measurements",
false);
151 tfL_ = std::make_unique<tf2_ros::TransformListener>(*
tf_);
152 tfB_ = std::make_unique<tf2_ros::TransformBroadcaster>();
162 pose_pub_ = node.advertise<geometry_msgs::PoseWithCovarianceStamped>(
"pose", 10,
true);
169 if(transform_publish_period == 0)
174 ros::Rate r(1.0 / transform_publish_period);
179 geometry_msgs::TransformStamped msg;
184 tfB_->sendTransform(msg);
194 nav_msgs::OccupancyGrid& og =
map_.map;
196 og.info.origin.position.x = 0.0;
197 og.info.origin.position.y = 0.0;
198 og.info.origin.position.z = 0.0;
199 og.info.origin.orientation.x = 0.0;
200 og.info.origin.orientation.y = 0.0;
201 og.info.origin.orientation.z = 0.0;
202 og.info.origin.orientation.w = 1.0;
205 double map_update_interval;
206 if(!
nh_.
getParam(
"map_update_interval", map_update_interval))
207 map_update_interval = 10.0;
227 geometry_msgs::Pose2D pose;
231 slam_toolbox_msgs::DeserializePoseGraph::Request req;
232 slam_toolbox_msgs::DeserializePoseGraph::Response resp;
233 req.initial_pose = pose;
238 slam_toolbox_msgs::DeserializePoseGraph::Request::START_AT_FIRST_NODE;
243 slam_toolbox_msgs::DeserializePoseGraph::Request::START_AT_GIVEN_POSE;
251 geometry_msgs::Pose2D& pose,
bool& start_at_dock)
257 std::vector<double> read_pose;
260 start_at_dock =
false;
261 if (read_pose.size() != 3)
263 ROS_ERROR(
"LocalizationSlamToolbox: Incorrect number of "
264 "arguments for map starting pose. Must be in format: "
265 "[x, y, theta]. Starting at the origin");
272 pose.x = read_pose[0];
273 pose.y = read_pose[1];
274 pose.theta = read_pose[2];
290 sensor_msgs::LaserScan::ConstPtr& scan)
293 const std::string& frame = scan->header.frame_id;
303 ROS_ERROR(
"Failed to compute laser pose, aborting initialization (%s)",
309 return lasers_[frame].getLaser();
344 const bool& update_reprocessing_transform)
356 geometry_msgs::TransformStamped base_to_map_msg, odom_to_map_msg;
363 ROS_ERROR(
"Transform from base_link to odom failed: %s", e.what());
371 if (update_reprocessing_transform)
379 odom_to_base_serialized * odom_to_base_current.
inverse();
385 tf2::Vector3( odom_to_map.getOrigin() ) ).
inverse();
393 const sensor_msgs::LaserScan::ConstPtr& scan,
399 *scan,
lasers_[scan->header.frame_id].isInverted());
416 const sensor_msgs::LaserScan::ConstPtr& scan,
422 static double min_dist2 =
423 smapper_->getMapper()->getParamMinimumTravelDistance() *
424 smapper_->getMapper()->getParamMinimumTravelDistance();
429 last_scan_time = scan->header.stamp;
455 if(dist2 < 0.8 * min_dist2 || scan->
header.seq < 5)
461 last_scan_time = scan->header.stamp;
469 PosedScan& scan_w_pose)
472 return addScan(laser, scan_w_pose.scan, scan_w_pose.pose);
478 const sensor_msgs::LaserScan::ConstPtr& scan,
484 laser, scan, karto_pose);
488 bool processed =
false, update_reprocessing_transform =
false;
495 processed =
smapper_->getMapper()->Process(range_scan, &covariance);
499 processed =
smapper_->getMapper()->ProcessAtDock(range_scan, &covariance);
501 update_reprocessing_transform =
true;
508 ROS_ERROR(
"Process near region called without a "
509 "valid region request. Ignoring scan.");
515 processed =
smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan,
false, &covariance);
516 update_reprocessing_transform =
true;
521 ROS_FATAL(
"SlamToolbox: No valid processor type set! Exiting.");
535 scan->header.stamp, update_reprocessing_transform);
542 range_scan =
nullptr;
555 geometry_msgs::PoseWithCovarianceStamped pose_msg;
556 pose_msg.header.stamp =
t;
575 nav_msgs::GetMap::Request &req,
576 nav_msgs::GetMap::Response &res)
579 if(
map_.map.info.width &&
map_.map.info.height)
593 slam_toolbox_msgs::Pause::Request& req,
594 slam_toolbox_msgs::Pause::Response& resp)
600 nh_.
setParam(
"paused_new_measurements", !curr_state);
601 ROS_INFO(
"SlamToolbox: Toggled to %s",
602 !curr_state ?
"pause taking new measurements." :
603 "actively taking new measurements.");
617 slam_toolbox_msgs::SerializePoseGraph::Request &req,
618 slam_toolbox_msgs::SerializePoseGraph::Response &resp)
621 std::string
filename = req.filename;
636 std::unique_ptr<karto::Mapper>& mapper,
637 std::unique_ptr<karto::Dataset>& dataset)
645 VerticeMap mapper_vertices = mapper->GetGraph()->GetVertices();
646 VerticeMap::iterator vertex_map_it = mapper_vertices.begin();
647 for(vertex_map_it; vertex_map_it != mapper_vertices.end(); ++vertex_map_it)
649 ScanMap::iterator vertex_it = vertex_map_it->second.begin();
650 for(vertex_it; vertex_it != vertex_map_it->second.end(); ++vertex_it)
652 if (vertex_it->second !=
nullptr)
654 solver_->AddNode(vertex_it->second);
659 EdgeVector mapper_edges = mapper->GetGraph()->GetEdges();
660 EdgeVector::iterator edges_it = mapper_edges.begin();
661 for( edges_it; edges_it != mapper_edges.end(); ++edges_it)
663 if (*edges_it !=
nullptr)
665 solver_->AddConstraint(*edges_it);
669 mapper->SetScanSolver(
solver_.get());
672 smapper_->setMapper(mapper.release());
680 ROS_FATAL(
"loadSerializedPoseGraph: Could not properly load "
681 "a valid mapping object. Did you modify something by hand?");
685 if (
dataset_->GetLasers().size() < 1)
687 ROS_FATAL(
"loadSerializedPoseGraph: Cannot deserialize "
688 "dataset with no laser objects.");
703 ROS_INFO(
"Waiting for incoming scan to get metadata...");
705 ros::topic::waitForMessage<sensor_msgs::LaserScan>(
712 lasers_[scan->header.frame_id] =
718 ROS_ERROR(
"Failed to compute laser pose, aborting continue mapping (%s)",
727 ROS_ERROR(
"Invalid sensor pointer in dataset. Unable to register sensor.");
737 slam_toolbox_msgs::DeserializePoseGraph::Request &req,
738 slam_toolbox_msgs::DeserializePoseGraph::Response &resp)
741 if (req.match_type == slam_toolbox_msgs::DeserializePoseGraph::Request::UNSET)
743 ROS_ERROR(
"Deserialization called without valid processor type set. "
744 "Undefined behavior!");
748 std::string
filename = req.filename;
762 std::unique_ptr<karto::Dataset> dataset = std::make_unique<karto::Dataset>();
763 std::unique_ptr<karto::Mapper> mapper = std::make_unique<karto::Mapper>();
767 ROS_ERROR(
"DeserializePoseGraph: Failed to read "
771 ROS_DEBUG(
"DeserializePoseGraph: Successfully read file.");
778 switch (req.match_type)
780 case procType::START_AT_FIRST_NODE:
783 case procType::START_AT_GIVEN_POSE:
786 req.initial_pose.y, req.initial_pose.theta);
788 case procType::LOCALIZE_AT_POSE:
791 req.initial_pose.y, req.initial_pose.theta);
794 ROS_FATAL(
"Deserialization called without valid processor type set.");