node.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 
00003 #include <boost/thread.hpp>
00004 #include "constants.h"
00005 #include "publisher.h"
00006 #include "tracking.h"
00007 #include "graph.h"
00008 #include "loop_closing.h"
00009 
00012 void readParameters(slam::Tracking::Params &tracking_params)
00013 {
00014   ros::NodeHandle nhp("~");
00015   nhp.param("odom_topic",   tracking_params.odom_topic,   string(""));
00016   nhp.param("range_topic",  tracking_params.range_topic,  string(""));
00017   nhp.param("camera_topic", tracking_params.camera_topic, string(""));
00018   nhp.param("min_range", tracking_params.min_range, 1.5);
00019   nhp.param("max_range", tracking_params.max_range, 3.5);
00020 }
00021 
00024 int main(int argc, char **argv)
00025 {
00026   // Override SIGINT handler
00027   ros::init(argc, argv, "stereo_slam");
00028   ros::start();
00029 
00030   // For debugging purposes
00031   slam::Publisher publisher;
00032 
00033   // Threads
00034   slam::LoopClosing loop_closing;
00035   slam::Graph graph(&loop_closing);
00036   slam::Tracking tracker(&publisher, &graph);
00037 
00038   // Read parameters
00039   slam::Tracking::Params tracking_params;
00040   readParameters(tracking_params);
00041 
00042   // Set the parameters for every object
00043   tracker.setParams(tracking_params);
00044   loop_closing.setGraph(&graph);
00045 
00046   // Launch threads
00047   boost::thread trackingThread(&slam::Tracking::run, &tracker);
00048   boost::thread graphThread(&slam::Graph::run, &graph);
00049   boost::thread loopClosingThread(&slam::LoopClosing::run, &loop_closing);
00050 
00051   // ROS spin
00052   ros::Rate r(10);
00053   while (ros::ok())
00054   {
00055     r.sleep();
00056   }
00057 
00058   // Loop closing object is the only one that needs finalization
00059   loop_closing.finalize();
00060 
00061   ros::shutdown();
00062 
00063   return 0;
00064 }


stereo_slam
Author(s): Pep Lluis Negre
autogenerated on Thu Jul 14 2016 04:09:13