mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 00:37:46 +08:00
119 lines
3.4 KiB
Diff
119 lines
3.4 KiB
Diff
diff --git a/src/slam_karto.cpp b/src/slam_karto.cpp
|
|
index 712a9ca..0c0d885 100644
|
|
--- a/src/slam_karto.cpp
|
|
+++ b/src/slam_karto.cpp
|
|
@@ -68,7 +68,7 @@ class SlamKarto
|
|
bool updateMap();
|
|
void publishTransform();
|
|
void publishLoop(double transform_publish_period);
|
|
- void publishGraphVisualization();
|
|
+ void publishGraphVisualization(const ros::Time & stamp);
|
|
|
|
// ROS handles
|
|
ros::NodeHandle node_;
|
|
@@ -435,16 +435,22 @@ SlamKarto::getOdomPose(karto::Pose2& karto_pose, const ros::Time& t)
|
|
}
|
|
|
|
void
|
|
-SlamKarto::publishGraphVisualization()
|
|
+SlamKarto::publishGraphVisualization(const ros::Time & stamp)
|
|
{
|
|
std::vector<float> graph;
|
|
solver_->getGraph(graph);
|
|
|
|
+ std::vector<karto::LocalizedRangeScan*> scans = mapper_->GetAllProcessedScans();
|
|
+
|
|
+ if(scans.empty())
|
|
+ {
|
|
+ return;
|
|
+ }
|
|
visualization_msgs::MarkerArray marray;
|
|
|
|
visualization_msgs::Marker m;
|
|
m.header.frame_id = "map";
|
|
- m.header.stamp = ros::Time::now();
|
|
+ m.header.stamp = stamp;
|
|
m.id = 0;
|
|
m.ns = "karto";
|
|
m.type = visualization_msgs::Marker::SPHERE;
|
|
@@ -462,7 +468,7 @@ SlamKarto::publishGraphVisualization()
|
|
|
|
visualization_msgs::Marker edge;
|
|
edge.header.frame_id = "map";
|
|
- edge.header.stamp = ros::Time::now();
|
|
+ edge.header.stamp = ros::Time(scans.back()->GetTime());
|
|
edge.action = visualization_msgs::Marker::ADD;
|
|
edge.ns = "karto";
|
|
edge.id = 0;
|
|
@@ -477,14 +483,14 @@ SlamKarto::publishGraphVisualization()
|
|
|
|
m.action = visualization_msgs::Marker::ADD;
|
|
uint id = 0;
|
|
- for (uint i=0; i<graph.size()/2; i++)
|
|
+ for (uint i=0; i<scans.size(); i++)
|
|
{
|
|
m.id = id;
|
|
- m.pose.position.x = graph[2*i];
|
|
- m.pose.position.y = graph[2*i+1];
|
|
+ m.pose.position.x = scans[i]->GetCorrectedPose().GetX();
|
|
+ m.pose.position.y = scans[i]->GetCorrectedPose().GetY();
|
|
marray.markers.push_back(visualization_msgs::Marker(m));
|
|
id++;
|
|
-
|
|
+/*
|
|
if(i>0)
|
|
{
|
|
edge.points.clear();
|
|
@@ -500,15 +506,15 @@ SlamKarto::publishGraphVisualization()
|
|
|
|
marray.markers.push_back(visualization_msgs::Marker(edge));
|
|
id++;
|
|
- }
|
|
+ }*/
|
|
}
|
|
-
|
|
+/*
|
|
m.action = visualization_msgs::Marker::DELETE;
|
|
for (; id < marker_count_; id++)
|
|
{
|
|
m.id = id;
|
|
marray.markers.push_back(visualization_msgs::Marker(m));
|
|
- }
|
|
+ }*/
|
|
|
|
marker_count_ = marray.markers.size();
|
|
|
|
@@ -537,12 +543,14 @@ SlamKarto::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
|
|
karto::Pose2 odom_pose;
|
|
if(addScan(laser, scan, odom_pose))
|
|
{
|
|
- ROS_DEBUG("added scan at pose: %.3f %.3f %.3f",
|
|
+ ROS_INFO("added scan at pose: %.3f %.3f %.3f",
|
|
odom_pose.GetX(),
|
|
odom_pose.GetY(),
|
|
odom_pose.GetHeading());
|
|
|
|
- publishGraphVisualization();
|
|
+ publishGraphVisualization(scan->header.stamp);
|
|
+
|
|
+ ROS_INFO("published markers");
|
|
|
|
if(!got_map_ ||
|
|
(scan->header.stamp - last_map_update) > map_update_interval_)
|
|
diff --git a/src/spa_solver.cpp b/src/spa_solver.cpp
|
|
index 5d9a962..6a65211 100644
|
|
--- a/src/spa_solver.cpp
|
|
+++ b/src/spa_solver.cpp
|
|
@@ -46,9 +46,9 @@ void SpaSolver::Compute()
|
|
|
|
typedef std::vector<sba::Node2d, Eigen::aligned_allocator<sba::Node2d> > NodeVector;
|
|
|
|
- ROS_INFO("Calling doSPA for loop closure");
|
|
+ //ROS_INFO("Calling doSPA for loop closure");
|
|
m_Spa.doSPA(40);
|
|
- ROS_INFO("Finished doSPA for loop closure");
|
|
+ //ROS_INFO("Finished doSPA for loop closure");
|
|
NodeVector nodes = m_Spa.getNodes();
|
|
forEach(NodeVector, &nodes)
|
|
{
|