05:55:04 PM

added resolution and load time to the map message;
06:47:47 PM
finally got real time map publishing to rviz working correctly;
08:31:00 PM
added initial code for Sending Goals to the Navigation Stack to the subscriber;

Examples/ROS/ORB_SLAM2/src/ros_mono_sub.cc

@@ -22,6 +22,11 @@
 #include <opencv2/highgui/highgui.hpp>
 #include <Converter.h>
 
+#include <move_base_msgs/MoveBaseAction.h>
+#include <actionlib/client/simple_action_client.h>
+
+typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
+
 // parameters
 float scale_factor = 3;
 float resize_factor = 5;
@@ -49,6 +54,17 @@ unsigned int n_kf_received;
 bool loop_closure_being_processed = false;
 ros::Publisher pub_grid_map;
 nav_msgs::OccupancyGrid grid_map_msg;
+#ifdef COMPILEDWITHC11
+std::chrono::steady_clock::time_point start_time, end_time;
+#else
+std::chrono::monotonic_clock::time_point start_time, end_time;
+#endif
+bool got_start_time;
+MoveBaseClient ac;
+move_base_msgs::MoveBaseGoal goal;
+
+float kf_pos_x, kf_pos_z;
+int kf_pos_grid_x, kf_pos_grid_z;
 
 using namespace std;
 
@@ -70,14 +86,26 @@ void processMapPts(const std::vector<geometry_msgs::Pose> &pts, unsigned int n_p
 	unsigned int start_id, int kf_pos_grid_x, int kf_pos_grid_z);
 void getGridMap();
 
-
 int main(int argc, char **argv){
 	ros::init(argc, argv, "Monosub");
 	ros::start();
 
+	//tell the action client that we want to spin a thread by default
+	ac = MoveBaseClient("move_base", true);
+
+	//wait for the action server to come up
+	while (!ac.waitForServer(ros::Duration(5.0))){
+		ROS_INFO("Waiting for the move_base action server to come up");
+	}
+
+	//we'll send a goal to the robot to move 1 meter forward
+	goal.target_pose.header.frame_id = "base_link";
+
 	parseParams(argc, argv);
 	printParams();
 
+	got_start_time = false;
+
 	grid_max_x = cloud_max_x*scale_factor;
 	grid_min_x = cloud_min_x*scale_factor;
 	grid_max_z = cloud_max_z*scale_factor;
@@ -99,8 +127,9 @@ int main(int argc, char **argv){
 	grid_map_msg.data.resize(h*w);
 	grid_map_msg.info.width = w;
 	grid_map_msg.info.height = h;
-	grid_map_int = cv::Mat(h, w, CV_8SC1, (char*)(grid_map_msg.data.data()));
+	grid_map_msg.info.resolution = 1.0/scale_factor;
 
+	grid_map_int = cv::Mat(h, w, CV_8SC1, (char*)(grid_map_msg.data.data()));
 
 	grid_map.create(h, w, CV_32FC1);
 	grid_map_thresh.create(h, w, CV_8UC1);
@@ -120,6 +149,8 @@ int main(int argc, char **argv){
 	ros::Subscriber sub_pts_and_pose = nodeHandler.subscribe("pts_and_pose", 1000, ptCallback);
 	ros::Subscriber sub_all_kf_and_pts = nodeHandler.subscribe("all_kf_and_pts", 1000, loopClosingCallback);
 	pub_grid_map = nodeHandler.advertise<nav_msgs::OccupancyGrid>("grid_map", 1000);
+
+
 	//ros::Subscriber sub_cloud = nodeHandler.subscribe("cloud_in", 1000, cloudCallback);
 	//ros::Subscriber sub_kf = nodeHandler.subscribe("camera_pose", 1000, kfCallback);
 	//ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage, &igb);
@@ -164,8 +195,39 @@ void ptCallback(const geometry_msgs::PoseArray::ConstPtr& pts_and_pose){
 	//	ros::shutdown();
 	//	exit(0);
 	//}
+//	if (!got_start_time) {
+//#ifdef COMPILEDWITHC11
+//		start_time = std::chrono::steady_clock::now();
+//#else
+//		start_time = std::chrono::monotonic_clock::now();
+//#endif
+//		got_start_time = true;
+//	}
+
 	if (loop_closure_being_processed){ return; }
+
 	updateGridMap(pts_and_pose);
+
+//#ifdef COMPILEDWITHC11
+//	end_time = std::chrono::steady_clock::now();
+//#else
+//	end_time = std::chrono::monotonic_clock::now();
+//#endif
+//	double curr_time = std::chrono::duration_cast<std::chrono::duration<double>>(start_time - end_time).count();
+
+	grid_map_msg.info.map_load_time = ros::Time::now();
+	pub_grid_map.publish(grid_map_msg);
+	goal.target_pose.header.stamp = ros::Time::now();
+	goal.target_pose.pose.position.x = kf_pos_grid_x;
+	goal.target_pose.pose.position.y = kf_pos_grid_z;
+	goal.target_pose.pose.orientation = pts_and_pose->poses[0].orientation;
+	ROS_INFO("Sending goal");
+	ac.sendGoal(goal);
+	ac.waitForResult();
+	if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
+		ROS_INFO("Hooray, the base moved 1 meter forward");
+	else
+		ROS_INFO("The base failed to move forward 1 meter for some reason");
 }
 void loopClosingCallback(const geometry_msgs::PoseArray::ConstPtr& all_kf_and_pts){
 	//ROS_INFO("Received points and pose: [%s]{%d}", pts_and_pose->header.frame_id.c_str(),
@@ -300,11 +362,11 @@ void updateGridMap(const geometry_msgs::PoseArray::ConstPtr& pts_and_pose){
 	const geometry_msgs::Point &kf_location = pts_and_pose->poses[0].position;
 	//const geometry_msgs::Quaternion &kf_orientation = pts_and_pose->poses[0].orientation;
 
-	float kf_pos_x = kf_location.x*scale_factor;
-	float kf_pos_z = kf_location.z*scale_factor;
+	kf_pos_x = kf_location.x*scale_factor;
+	kf_pos_z = kf_location.z*scale_factor;
 
-	int kf_pos_grid_x = int(floor((kf_pos_x - grid_min_x) * norm_factor_x));
-	int kf_pos_grid_z = int(floor((kf_pos_z - grid_min_z) * norm_factor_z));
+	kf_pos_grid_x = int(floor((kf_pos_x - grid_min_x) * norm_factor_x));
+	kf_pos_grid_z = int(floor((kf_pos_z - grid_min_z) * norm_factor_z));
 
 	if (kf_pos_grid_x < 0 || kf_pos_grid_x >= w)
 		return;
@@ -375,12 +437,10 @@ void resetGridMap(const geometry_msgs::PoseArray::ConstPtr& all_kf_and_pts){
 #endif
 	double ttrack = std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
 	printf("Done. Time taken: %f secs\n", ttrack);
-	grid_map_int = grid_map * 100;	
 	pub_grid_map.publish(grid_map_msg);
 	showGridMap(all_kf_and_pts->header.seq);
 }
 
-
 void getGridMap() {
 	for (int row = 0; row < h; ++row){
 		for (int col = 0; col < w; ++col){
@@ -402,13 +462,15 @@ void getGridMap() {
 			else {
 				grid_map_thresh.at<uchar>(row, col) = 0;
 			}
+			grid_map_int.at<char>(row, col) = (1 - grid_map.at<float>(row, col)) * 100;
 		}
 	}
 	cv::resize(grid_map_thresh, grid_map_thresh_resized, grid_map_thresh_resized.size());
 }
 void showGridMap(unsigned int id) {
+	cv::imshow("grid_map_msg", cv::Mat(h, w, CV_8SC1, (char*)(grid_map_msg.data.data())));
 	cv::imshow("grid_map_thresh_resized", grid_map_thresh_resized);
-	cv::imshow("grid_map", grid_map);
+	//cv::imshow("grid_map", grid_map);
 	int key = cv::waitKey(1) % 256;
 	if (key == 27) {
 		cv::destroyAllWindows();

grid_map.rviz

@@ -0,0 +1,120 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 81
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Map1
+        - /Map1/Status1
+      Splitter Ratio: 0.5
+    Tree Height: 535
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 0.7
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: grid_map
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 11.8163
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 0.785398
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.785398
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002a9fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006700fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000043800000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000043000002a9000000e300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002a9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000043000002a9000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000023a00fffffffb0000000800540069006d006501000000000000045000000000000000000000022b000002a900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1200
+  X: 2308
+  Y: 1208

src/LoopClosing.cc

@@ -401,8 +401,7 @@ bool LoopClosing::ComputeSim3()
 
 void LoopClosing::CorrectLoop()
 {
-	mpTracker->loop_detected = true;
-	loop_detected = true;
+
 	cout << "Loop detected!" << endl;
 
     // Send a stop signal to Local Mapping
@@ -748,6 +747,7 @@ void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF)
         mbFinishedGBA = true;
         mbRunningGBA = false;
     }
+	loop_detected = mpTracker->loop_detected = true;
 }
 
 void LoopClosing::RequestFinish()

steps

@@ -77,16 +77,19 @@ rosrun ORB_SLAM2 Monopub Vocabulary/ORBvoc.txt Examples/Monocular/demo_cam.yaml
 
 start subscriber on kitti 00:
 rosrun ORB_SLAM2 Monosub 5 3 29 -25 48 -12 0.55 0.50 1 5
-rosrun ORB_SLAM2 Monosub 100 0.2 29 -25 48 -12 0.55 0.50 1 5
+rosrun ORB_SLAM2 Monosub 10 1 29 -25 48 -12 0.55 0.50 1 5
 
 start subscriber on rosbag:
 rosrun ORB_SLAM2 Monosub 30 5 2 -2 2 -2 0.55 0.50 1 5
 rosrun ORB_SLAM2 Monosub 30 2 6 -6 6 -6 0.55 0.50 1 5
+rosrun ORB_SLAM2 Monosub 10 0.5 20 -10 20 -10 0.55 0.50 1 5
 
 running rosbag of recording:
 rosbag play "/media/abhineet/Win 8/bags/2017-04-03-20-35-36.bag" -r 0.5
+rosbag play 2017-04-09-22-08-04.bag -r 0.5
 rosbag play "/media/abhineet/Win 8/bags/2017-04-03-13-37-07.bag" -r 0.5
 
-
+running rviz with preloaded map display and topic name:
+rosrun rviz rviz -d grid_map.rviz