Testing done with good results

patweatharva · May 15, 2024 · d76aeed · d76aeed
1 parent 33eb10b
commit d76aeed
Show file tree

Hide file tree

Showing 7 changed files with 28,400 additions and 38,455 deletions.
diff --git a/include/graph_slam_node.hpp b/include/graph_slam_node.hpp
@@ -316,7 +316,7 @@ void graph_slam_handler::scanCB(const turtlebot_graph_slam::tfArrayConstPtr &sca
                     // add initial estimates composing logic
                     if (i == 0)
                     {
-                        current_pose_ = current_pose_.compose(odometry);
+                        current_pose_ = current_pose_.compose(scan_pose);
                         initial_estimates_.insert(X(index_), current_pose_);
 
                         // add odom factor to the graph

diff --git a/include/scan_match_node.hpp b/include/scan_match_node.hpp
@@ -207,8 +207,8 @@ class ScanHandler
             try
             {
                 ros::Time start_time_ = ros::Time::now();
-                // projector_.transformLaserScanToPointCloud("turtlebot/kobuki/base_footprint", *scan, cloud_in, listener_);
-                projector_.projectLaser(*scan,cloud_in);
+                projector_.transformLaserScanToPointCloud("turtlebot/kobuki/base_footprint", *scan, cloud_in, listener_);
+                // projector_.projectLaser(*scan,cloud_in);
 
                 current_scan_odom_ = current_odom_;
                 current_scan_odom_.header.frame_id = std::to_string(current_scan_index);

diff --git a/launch/env.launch b/launch/env.launch
@@ -3,7 +3,7 @@
     <rosparam file="$(find turtlebot_graph_slam)/config/params.yaml" command="load"/>
 
     <!-- StoneFish simulator -->
-    <include file="$(find turtlebot_simulation)/launch/turtlebot_integration.launch" />
+    <!-- <include file="$(find turtlebot_simulation)/launch/turtlebot_integration.launch" /> -->
 
     <!-- Environment node -->
     <node pkg="turtlebot_graph_slam" type="turtlebot_teleop_convert_node.py" name="turtlebot_teleop_convert_node"/>