updated goal action feedback

launch/azimut3/config/azimut3_nav.rviz

@@ -6,9 +6,6 @@ Panels:
       Expanded:
         - /Global Options1
         - /TF1/Frames1
-        - /MapCloud1
-        - /Path1
-        - /Path2
       Splitter Ratio: 0.601881
     Tree Height: 192
   - Class: rviz/Selection
@@ -22,7 +19,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: ""
+    SyncSource: Info
   - Class: rviz/Tool Properties
     Expanded:
       - /2D Pose Estimate1
@@ -71,6 +68,8 @@ Visualization Manager:
           Value: true
         camera_rgb_optical_frame:
           Value: true
+        map:
+          Value: true
         odom:
           Value: true
         wheelLB_linkWheel_link:
@@ -95,7 +94,31 @@ Visualization Manager:
       Show Axes: true
       Show Names: true
       Tree:
-        {}
+        map:
+          odom:
+            base_footprint:
+              base_link:
+                base_laser_link:
+                  {}
+                camera_link:
+                  camera_depth_frame:
+                    camera_depth_optical_frame:
+                      {}
+                  camera_rgb_frame:
+                    camera_rgb_optical_frame:
+                      {}
+                wheelLB_linkWheel_link:
+                  wheelLB_wheel_link:
+                    {}
+                wheelLF_linkWheel_link:
+                  wheelLF_wheel_link:
+                    {}
+                wheelRB_linkWheel_link:
+                  wheelRB_wheel_link:
+                    {}
+                wheelRF_linkWheel_link:
+                  wheelRF_wheel_link:
+                    {}
       Update Interval: 0
       Value: true
     - Alpha: 1
@@ -139,18 +162,18 @@ Visualization Manager:
       Class: rviz/Map
       Color Scheme: costmap
       Draw Behind: false
-      Enabled: true
+      Enabled: false
       Name: Map
       Topic: /planner/move_base/local_costmap/costmap
-      Value: true
+      Value: false
     - Alpha: 0.7
       Class: rviz/Map
       Color Scheme: costmap
       Draw Behind: false
-      Enabled: false
+      Enabled: true
       Name: Map
       Topic: /planner/move_base/global_costmap/costmap
-      Value: false
+      Value: true
     - Alpha: 1
       Class: rviz/RobotModel
       Collision Enabled: false
@@ -284,7 +307,7 @@ Visualization Manager:
       Shaft Length: 1
       Shaft Radius: 0.05
       Shape: Arrow
-      Topic: /planner_goal
+      Topic: /rtabmap/goal
       Value: true
     - Alpha: 1
       Buffer Length: 1
@@ -333,11 +356,27 @@ Visualization Manager:
       Value: false
     - Alpha: 1
       Class: rtabmap_ros/MapGraph
-      Color: 255; 0; 0
+      Color: 0; 0; 255
       Enabled: true
       Name: MapGraph
       Topic: /rtabmap/mapData_relay
       Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 255; 85; 255
+      Enabled: true
+      Name: Path
+      Topic: /rtabmap/global_path
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 85; 255; 255
+      Enabled: true
+      Name: Path
+      Topic: /rtabmap/local_path
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -358,7 +397,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 8.53098
+      Distance: 7.4465
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.06
         Stereo Focal Distance: 1
@@ -370,10 +409,10 @@ Visualization Manager:
         Z: -0.156054
       Name: Current View
       Near Clip Distance: 0.01
-      Pitch: 0.9304
+      Pitch: 1.4204
       Target Frame: base_footprint
       Value: Orbit (rviz)
-      Yaw: 2.26538
+      Yaw: 2.93539
     Saved: ~
 Window Geometry:
   Displays:
@@ -393,5 +432,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 714
-  X: 928
+  X: 921
   Y: 32

src/CoreWrapper.cpp

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <cv_bridge/cv_bridge.h>
 #include <nav_msgs/Path.h>
 #include <std_msgs/Int32MultiArray.h>
+#include <std_msgs/Bool.h>
 #include <rtabmap/core/RtabmapEvent.h>
 #include <rtabmap/core/Camera.h>
 #include <rtabmap/core/Parameters.h>
@@ -138,7 +139,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	goalSub_ = nh.subscribe("goal", 1, &CoreWrapper::goalCallback, this);
 	goalGlobalSub_ = nh.subscribe("goal_global", 1, &CoreWrapper::goalGlobalCallback, this);
 	nextMetricGoalPub_ = nh.advertise<geometry_msgs::PoseStamped>("goal_out", 1);
-	goalReachedPub_ = nh.advertise<std_msgs::Empty>("goal_reached", 1);
+	goalReachedPub_ = nh.advertise<std_msgs::Bool>("goal_reached", 1);
 	globalPathPub_ = nh.advertise<nav_msgs::Path>("global_path", 1);
 	localPathPub_ = nh.advertise<nav_msgs::Path>("local_path", 1);
 
@@ -945,7 +946,9 @@ void CoreWrapper::process(
 					ROS_INFO("Planning: Publishing goal reached!");
 					if(goalReachedPub_.getNumSubscribers())
 					{
-						goalReachedPub_.publish(std_msgs::Empty());
+						std_msgs::Bool result;
+						result.data = true;
+						goalReachedPub_.publish(result);
 					}
 					currentMetricGoal_.setNull();
 				}
@@ -1005,6 +1008,12 @@ void CoreWrapper::goalCommonCallback(const std::list<std::pair<int, Transform> >
 		{
 			ROS_ERROR("Pose of node %d not found!? Cannot send a metric goal...", rtabmap_.getPathCurrentGoalId());
 			rtabmap_.clearPath();
+			if(goalReachedPub_.getNumSubscribers())
+			{
+				std_msgs::Bool result;
+				result.data = false;
+				goalReachedPub_.publish(result);
+			}
 		}
 		else
 		{
@@ -1044,7 +1053,13 @@ void CoreWrapper::goalCommonCallback(const std::list<std::pair<int, Transform> >
 	else
 	{
 		ROS_WARN("Planning: Cannot compute a path (or goal is already reached)!");
-		goalReachedPub_.publish(std_msgs::Empty());
+		rtabmap_.clearPath();
+		if(goalReachedPub_.getNumSubscribers())
+		{
+			std_msgs::Bool result;
+			result.data = false;
+			goalReachedPub_.publish(result);
+		}
 	}
 }
 
@@ -1428,25 +1443,34 @@ void CoreWrapper::goalDoneCb(const actionlib::SimpleClientGoalState& state,
 {
 	if(state == actionlib::SimpleClientGoalState::SUCCEEDED)
 	{
-		ROS_INFO("Planning: move_base success");
+		ROS_INFO("Planning: move_base success!");
 	}
 	else
 	{
-		ROS_INFO("Planning: move_base failed for some reason.");
+		ROS_ERROR("Planning: move_base failed for some reason. Aborting the plan...");
+	}
+
+	rtabmap_.clearPath();
+	currentMetricGoal_.setNull();
+	if(goalReachedPub_.getNumSubscribers())
+	{
+		std_msgs::Bool result;
+		result.data = state == actionlib::SimpleClientGoalState::SUCCEEDED;
+		goalReachedPub_.publish(result);
 	}
 }
 
 // Called once when the goal becomes active
 void CoreWrapper::goalActiveCb()
 {
-	ROS_INFO("Planning: Goal just went active");
+	//ROS_INFO("Planning: Goal just went active");
 }
 
 // Called every time feedback is received for the goal
 void CoreWrapper::goalFeedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback)
 {
-	Transform basePosition = rtabmap_ros::transformFromPoseMsg(feedback->base_position.pose);
+	//Transform basePosition = rtabmap_ros::transformFromPoseMsg(feedback->base_position.pose);
-	ROS_INFO("Planning: feedback base_position = %s", basePosition.prettyPrint().c_str());
+	//ROS_INFO("Planning: feedback base_position = %s", basePosition.prettyPrint().c_str());
 }
 
 void CoreWrapper::publishLocalPath(const ros::Time & stamp)