Pose goals can be sent directly to rtabmap node, Added SetGoal service (set target node id)

CMakeLists.txt

@@ -52,6 +52,7 @@ add_message_files(
    GetMap.srv
    PublishMap.srv
    ResetPose.srv
+   SetGoal.srv
  )
 
 ## Generate added messages and services with any dependencies listed here

launch/azimut3/az3_mapping_client_nav.launch

@@ -17,21 +17,8 @@
   <!-- Visualisation RVIZ -->
   <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/azimut3/config/azimut3_nav.rviz"/>
     
-  <!-- Below, construct point cloud of the latest throttled data, disabled for bandwidth efficiency -->
-  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
-    <remap from="rgb/image"       to="/camera/data_throttled_image_relay"/>
-    <remap from="depth/image"     to="/camera/data_throttled_image_depth_relay"/>
-    <remap from="rgb/camera_info" to="/camera/data_throttled_camera_info_relay"/>
-    <remap from="cloud"           to="voxel_cloud" />
-    
-    <param name="queue_size" type="int" value="10"/>
-    <param name="voxel_size" type="double" value="0.01"/>
-  </node>
-  
   <!-- use a relay on this machine, same for images -->
-  <node if="$(arg rviz)" name="mapData_relay" type="relay" pkg="topic_tools" args="/rtabmap/mapData /rtabmap/mapData_relay"/>
+  <node if="$(arg rviz)" name="mapData_relay" type="relay" pkg="topic_tools" args="/rtabmap/mapData /rtabmap/mapData_relay">
-  <node if="$(arg rviz)" name="camera_info_relay" type="relay" pkg="topic_tools" args="/camera/data_throttled_camera_info /camera/data_throttled_camera_info_relay"/>
+    <param name="lazy" type="bool" value="true"/>
-  <node if="$(arg rviz)" name="republish_rgb" type="republish" pkg="image_transport" args="theora in:=/camera/data_throttled_image raw out:=/camera/data_throttled_image_relay" />
+  </node>
-  <node if="$(arg rviz)" name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=/camera/data_throttled_image_depth raw out:=/camera/data_throttled_image_depth_relay" />
-  
 </launch>

launch/azimut3/az3_mapping_robot_nav.launch

@@ -7,73 +7,9 @@
   <!-- AZIMUT 3 bringup: launch motors/odometry, laser scan and openni -->
   <include file="$(find az3_bringup)/az3_standalone.launch"/>
 
-  <node name="joy" pkg="joy" type="joy_node"/>
-
-  <group ns="teleop">
-        <remap from="joy" to="/joy"/>
-        <node name="teleop" pkg="nodelet" type="nodelet" 
-            args="standalone azimut_tools/Teleop"/>
-        <param name="cmd_eta/abtr_priority" value="50"/>
-    </group>
-
-    <group ns="go_back">
-        <remap from="joy"  to="/joy"/>
-        <remap from="goal" to="/planner_goal"/>
-        <node name="go_back" pkg="azimut_tools" type="go_back"/>
-        <rosparam>
-            btn_save: 1
-            btn_goto: 3
-        </rosparam>
-    </group>
-
-    <group ns="planner">
-        <remap from="openni_points" to="/planner_cloud"/>
-        <remap from="base_scan" to="/base_scan"/>
-        <remap from="map" to="/map"/>
-        <remap from="move_base_simple/goal" to="/planner_goal"/>
-        <include file="$(find az3_navigation)/launch/az3_move_base_slam.launch"/>
-        <param name="cmd_vel/abtr_priority" value="10"/>
-    </group>
-
-    <node name="az3_abtr" pkg="azimut_tools" type="azimut_abtr_priority_node">
-        <remap from="abtr_cmd_eta" to="/base_controller/cmd_eta"/>
-    </node>
-
-    <node name="register_cmd_eta" pkg="abtr_priority" type="register"
-        args="/cmd_eta /teleop/cmd_eta"/>
-    <node name="register_cmd_vel" pkg="abtr_priority" type="register"
-        args="/cmd_vel /planner/cmd_vel"/>
-
-
   <!-- OpenNI -->
   <include file="$(find rtabmap_ros)/launch/azimut3/az3_openni.launch"/>
 
-  <!-- Throttling messages -->
-  <group ns="camera">
-    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
-      <param name="max_rate" type="double" value="5.0"/>
-   
-      <remap from="rgb/image_in"       to="rgb/image_rect_color"/>
-      <remap from="depth/image_in"     to="depth_registered/image_raw"/>
-      <remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
-    
-      <remap from="rgb/image_out"       to="data_throttled_image"/>
-      <remap from="depth/image_out"     to="data_throttled_image_depth"/>
-      <remap from="rgb/camera_info_out" to="data_throttled_camera_info"/>
-    </node>
-
-    <!-- for the planner -->
-    <node pkg="nodelet" type="nodelet" name="points_xyzrgb_planner" args="load rtabmap_ros/point_cloud_xyzrgb camera_nodelet_manager">
-      <remap from="rgb/image"       to="/camera/data_throttled_image"/>
-      <remap from="depth/image"     to="/camera/data_throttled_image_depth"/>
-      <remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
-      <remap from="cloud"           to="/planner_cloud" />
-    
-      <param name="queue_size" type="int" value="10"/>
-      <param name="decimation" type="int" value="4"/>
-    </node>  
-  </group>  
-
   <!-- SLAM (robot side) -->
   <group ns="rtabmap">
     <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
@@ -94,7 +30,6 @@
 
       <param name="queue_size" type="int" value="10"/>
 	
-      <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
       <param name="RGBD/PoseScanMatching" type="string" value="true"/>           
       <param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/>
   
@@ -136,4 +71,41 @@
     </node>
 
   </group>
+  
+  <!-- teleop -->
+  <node name="joy" pkg="joy" type="joy_node"/>
+  <group ns="teleop">
+     <remap from="joy" to="/joy"/>
+     <node name="teleop" pkg="nodelet" type="nodelet" args="standalone azimut_tools/Teleop"/>
+     <param name="cmd_eta/abtr_priority" value="50"/>
+  </group>
+
+  <!-- ROS navigation stack move_base -->
+  <group ns="planner">
+     <remap from="openni_points" to="/planner_cloud"/>
+     <remap from="base_scan" to="/base_scan"/>
+     <remap from="map" to="/map"/>
+     <remap from="move_base_simple/goal" to="/planner_goal"/>
+        
+     <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
+     	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
+    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/local_costmap_params_laser_only.yaml" command="load" />
+    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/global_costmap_params.yaml" command="load" />
+    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/base_local_planner_params.yaml" command="load" />
+     </node>
+   		
+     <param name="cmd_vel/abtr_priority" value="10"/>
+  </group>
+
+  <node name="az3_abtr" pkg="azimut_tools" type="azimut_abtr_priority_node">
+     <remap from="abtr_cmd_eta" to="/base_controller/cmd_eta"/>
+  </node>
+
+  <!-- Arbitration between teleop and planner -->
+  <node name="register_cmd_eta" pkg="abtr_priority" type="register"
+        args="/cmd_eta /teleop/cmd_eta"/>
+  <node name="register_cmd_vel" pkg="abtr_priority" type="register"
+        args="/cmd_vel /planner/cmd_vel"/>
+  
 </launch>

launch/azimut3/config/azimut3_nav.rviz

@@ -6,23 +6,23 @@ Panels:
       Expanded:
         - /Global Options1
         - /TF1/Frames1
-        - /Info1/Status1
+        - /MapCloud1
-        - /MapGraph1
+        - /Path1
+        - /Path2
       Splitter Ratio: 0.601881
-    Tree Height: 337
+    Tree Height: 437
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Views
     Expanded:
       - /Current View1
-      - /Current View1/Focal Point1
     Name: Views
     Splitter Ratio: 0.5
   - Class: rviz/Time
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: Image
+    SyncSource: ""
   - Class: rviz/Tool Properties
     Expanded:
       - /2D Pose Estimate1
@@ -127,8 +127,8 @@ Visualization Manager:
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 5.19727
+        Max Value: 3.46596
-        Min Value: -0.547517
+        Min Value: -5.27159e-08
         Value: true
       Axis: X
       Channel Name: intensity
@@ -136,7 +136,7 @@ Visualization Manager:
       Color: 255; 255; 255
       Color Transformer: AxisColor
       Decay Time: 0
-      Enabled: true
+      Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Max Intensity: 4096
@@ -152,7 +152,7 @@ Visualization Manager:
       Topic: /base_scan
       Use Fixed Frame: false
       Use rainbow: true
-      Value: true
+      Value: false
     - Alpha: 0.7
       Class: rviz/Map
       Color Scheme: map
@@ -173,10 +173,10 @@ Visualization Manager:
       Class: rviz/Map
       Color Scheme: costmap
       Draw Behind: false
-      Enabled: true
+      Enabled: false
       Name: Map
       Topic: /planner/move_base/global_costmap/costmap
-      Value: true
+      Value: false
     - Alpha: 1
       Class: rviz/RobotModel
       Collision Enabled: false
@@ -249,7 +249,7 @@ Visualization Manager:
       Value: true
       Visual Enabled: true
     - Class: rviz/Image
-      Enabled: true
+      Enabled: false
       Image Topic: /camera/data_throttled_image_relay
       Max Value: 1
       Median window: 5
@@ -258,7 +258,7 @@ Visualization Manager:
       Normalize Range: true
       Queue Size: 2
       Transport Hint: raw
-      Value: true
+      Value: false
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
@@ -384,32 +384,32 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 14.3083
+      Distance: 3.44552
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.06
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 1.91382
+        X: 0.0430748
-        Y: -0.564774
+        Y: -0.00858197
-        Z: 0.661231
+        Z: -0.156054
       Name: Current View
       Near Clip Distance: 0.01
-      Pitch: 1.4054
+      Pitch: 0.9304
-      Target Frame: <Fixed Frame>
+      Target Frame: base_footprint
       Value: Orbit (rviz)
-      Yaw: 3.1704
+      Yaw: 2.26538
     Saved: ~
 Window Geometry:
   Displays:
-    collapsed: false
+    collapsed: true
-  Height: 826
+  Height: 504
-  Hide Left Dock: false
+  Hide Left Dock: true
   Hide Right Dock: false
   Image:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000151000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000192000000dd00fffffffb0000000a0049006d00610067006501000001c00000015c0000001600fffffffb0000000a0049006d0061006700650000000184000000490000000000000000fb0000000a0049006d006100670065010000027d000000fa0000000000000000fb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000002f7000000800000006400ffffff000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002f4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003ad000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001510000028ffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000280000028f000000dd00fffffffb0000000a0049006d006100670065000000018a0000012d0000001600fffffffb0000000a0049006d0061006700650000000184000000490000000000000000fb0000000a0049006d006100670065010000027d000000fa0000000000000000fb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000002f7000000800000006400ffffff000000010000010f000001b2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000001b2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000002ca000001b200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -418,6 +418,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1561
+  Width: 714
-  X: 42
+  X: 935
-  Y: 53
+  Y: 32

launch/azimut3/config/local_costmap_params_laser_only.yaml

@@ -0,0 +1,33 @@
+local_costmap:
+  global_frame: map
+  robot_base_frame: base_footprint
+  update_frequency: 2.0
+  publish_frequency: 2.0
+  static_map: false
+  rolling_window: true
+  width: 4.0
+  height: 4.0
+  resolution: 0.025
+  origin_x: -2.0
+  origin_y: -2.0
+
+  observation_sources: laser_scan_sensor
+  #observation_sources: laser_scan_sensor point_cloud_sensor
+
+  laser_scan_sensor: {
+    data_type: LaserScan, 
+    topic: base_scan, 
+    expected_update_rate: 0.2, 
+    marking: true, 
+    clearing: true}
+
+  # assuming receiving a cloud from rtabmap/obstacles_detection node
+  #point_cloud_sensor: {
+  #  sensor_frame: base_footprint,
+  #  data_type: PointCloud2, 
+  #  topic: openni_points, 
+  #  expected_update_rate: 0.5, 
+  #  marking: true, 
+  #  clearing: true,
+  #  min_obstacle_height: -99999.0,
+  #  max_obstacle_height: 99999.0}

src/CoreWrapper.cpp

@@ -132,8 +132,9 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	mapGraph_ = nh.advertise<rtabmap_ros::Graph>("graph", 1);
 
 	// planning topics
-	goalNodeSub_ = nh.subscribe("goal_node", 1, &CoreWrapper::goalNodeCallback, this);
+	goalSub_ = nh.subscribe("in_goal", 1, &CoreWrapper::goalCallback, this);
-	nextMetricGoalPub_ = nh.advertise<geometry_msgs::PoseStamped>("goal_pose", 1);
+	goalGlobalSub_ = nh.subscribe("in_goal_global", 1, &CoreWrapper::goalGlobalCallback, this);
+	nextMetricGoalPub_ = nh.advertise<geometry_msgs::PoseStamped>("out_goal", 1);
 	goalReachedPub_ = nh.advertise<std_msgs::Empty>("goal_reached", 1);
 	globalPathPub_ = nh.advertise<nav_msgs::Path>("global_path", 1);
 	localPathPub_ = nh.advertise<nav_msgs::Path>("local_path", 1);
@@ -280,6 +281,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	setModeMappingSrv_ = nh.advertiseService("set_mode_mapping", &CoreWrapper::setModeMappingCallback, this);
 	getMapDataSrv_ = nh.advertiseService("get_map", &CoreWrapper::getMapCallback, this);
 	publishMapDataSrv_ = nh.advertiseService("publish_map", &CoreWrapper::publishMapCallback, this);
+	setGoalSrv_ = nh.advertiseService("set_goal", &CoreWrapper::setGoalCallback, this);
 
 	setupCallbacks(subscribeDepth, subscribeLaserScan, subscribeStereo, queueSize, stereoApproxSync);
 
@@ -946,16 +948,22 @@ void CoreWrapper::process(
 				}
 				else
 				{
-					Transform updatedGoalPose = rtabmap_.getPose(rtabmap_.getPathGoalId());
+					Transform updatedGoalPose = rtabmap_.getPose(rtabmap_.getPathCurrentGoalId());
 					if(!updatedGoalPose.isNull())
 					{
+						// Adjust the target pose relative to last node
+						if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back())
+						{
+							updatedGoalPose *= rtabmap_.getPathTransformToGoal();
+						}
+
 						// detect if the goal has changed or local map
 						// has changed so much that current goal drifted
 						if(currentMetricGoal_.getDistance(updatedGoalPose) > rtabmap_.getGoalReachedRadius()/2.0f)
 						{
 							currentMetricGoal_ = updatedGoalPose;
 
-							publishGoal(timeNow);
+							publishCurrentGoal(timeNow);
 						}
 
 						// publish local path
@@ -963,7 +971,7 @@ void CoreWrapper::process(
 					}
 					else
 					{
-						ROS_ERROR("Planning: Pose of node %d not found!? Cannot send a metric goal...", rtabmap_.getPathGoalId());
+						ROS_ERROR("Planning: Pose of node %d not found!? Cannot send a metric goal...", rtabmap_.getPathCurrentGoalId());
 					}
 				}
 			}
@@ -984,19 +992,15 @@ void CoreWrapper::process(
 			rtabmap_.getWMSize()+rtabmap_.getSTMSize());
 }
 
-void CoreWrapper::goalNodeCallback(const std_msgs::Int32ConstPtr & msg)
+void CoreWrapper::goalCommonCallback(const std::list<std::pair<int, Transform> > & poses)
 {
 	currentMetricGoal_.setNull();
-	int id = msg->data;
-	ROS_INFO("Planning: set goal %d", id);
-
-	std::list<std::pair<int, Transform> > poses = rtabmap_.computePath(id);
 	if(poses.size())
 	{
-		currentMetricGoal_ = rtabmap_.getPose(rtabmap_.getPathGoalId());
+		currentMetricGoal_ = rtabmap_.getPose(rtabmap_.getPathCurrentGoalId());
 		if(currentMetricGoal_.isNull())
 		{
-			ROS_ERROR("Pose of node %d not found!? Cannot send a metric goal...", rtabmap_.getPathGoalId());
+			ROS_ERROR("Pose of node %d not found!? Cannot send a metric goal...", rtabmap_.getPathCurrentGoalId());
 			rtabmap_.clearPath();
 		}
 		else
@@ -1013,7 +1017,7 @@ void CoreWrapper::goalNodeCallback(const std_msgs::Int32ConstPtr & msg)
 				path.poses.resize(poses.size());
 				int oi = 0;
 				std::stringstream stream;
-				for(std::list<std::pair<int, Transform> >::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+				for(std::list<std::pair<int, Transform> >::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 				{
 					path.poses[oi].header = path.header;
 					rtabmap_ros::transformToPoseMsg(iter->second, path.poses[oi].pose);
@@ -1024,7 +1028,13 @@ void CoreWrapper::goalNodeCallback(const std_msgs::Int32ConstPtr & msg)
 				globalPathPub_.publish(path);
 			}
 
-			publishGoal(now);
+			// Adjust the target pose relative to last node
+			if(rtabmap_.getPathCurrentGoalId() == poses.back().first)
+			{
+				currentMetricGoal_ *= rtabmap_.getPathTransformToGoal();
+			}
+
+			publishCurrentGoal(now);
 			publishLocalPath(now);
 		}
 	}
@@ -1035,6 +1045,30 @@ void CoreWrapper::goalNodeCallback(const std_msgs::Int32ConstPtr & msg)
 	}
 }
 
+void CoreWrapper::goalCallback(const geometry_msgs::PoseStampedConstPtr & msg)
+{
+	Transform targetPose = rtabmap_ros::transformFromPoseMsg(msg->pose);
+	if(targetPose.isNull())
+	{
+		ROS_ERROR("Pose received is null!");
+		return;
+	}
+	ROS_INFO("Planning: set goal %s", targetPose.prettyPrint().c_str());
+	goalCommonCallback(rtabmap_.computePath(targetPose, false));
+}
+
+void CoreWrapper::goalGlobalCallback(const geometry_msgs::PoseStampedConstPtr & msg)
+{
+	Transform targetPose = rtabmap_ros::transformFromPoseMsg(msg->pose);
+	if(targetPose.isNull())
+	{
+		ROS_ERROR("Pose received is null!");
+		return;
+	}
+	ROS_INFO("Planning: set goal %s", targetPose.prettyPrint().c_str());
+	goalCommonCallback(rtabmap_.computePath(targetPose, true));
+}
+
 bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
 {
 	rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
@@ -1278,6 +1312,14 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 	return true;
 }
 
+bool CoreWrapper::setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res)
+{
+	int id = req.target_node_id;
+	ROS_INFO("Planning: set goal %d", id);
+	goalCommonCallback(rtabmap_.computePath(id, req.in_global_graph));
+	return !currentMetricGoal_.isNull();
+}
+
 void CoreWrapper::publishStats(const Statistics & stats, const ros::Time & stamp)
 {
 	if(infoPub_.getNumSubscribers())
@@ -1331,19 +1373,19 @@ void CoreWrapper::publishStats(const Statistics & stats, const ros::Time & stamp
 	}
 }
 
-void CoreWrapper::publishGoal(const ros::Time & stamp)
+void CoreWrapper::publishCurrentGoal(const ros::Time & stamp)
 {
 	if(!currentMetricGoal_.isNull())
 	{
 		ROS_INFO("Planning: Publishing next goal: Location %d pose=%s",
-				rtabmap_.getPathGoalId(), currentMetricGoal_.prettyPrint().c_str());
+				rtabmap_.getPathCurrentGoalId(), currentMetricGoal_.prettyPrint().c_str());
 		if(nextMetricGoalPub_.getNumSubscribers())
 		{
 			geometry_msgs::PoseStamped goalMsg;
 			goalMsg.header.frame_id = mapFrameId_;
 			goalMsg.header.stamp = ros::Time::now();
 			rtabmap_ros::transformToPoseMsg(currentMetricGoal_, goalMsg.pose);
-			ROS_INFO("Publishing next goal: %d", rtabmap_.getPathGoalId());
+			ROS_INFO("Publishing next goal: %d", rtabmap_.getPathCurrentGoalId());
 			nextMetricGoalPub_.publish(goalMsg);
 		}
 	}

src/CoreWrapper.h

@@ -53,6 +53,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap_ros/GetMap.h"
 #include "rtabmap_ros/PublishMap.h"
+#include "rtabmap_ros/SetGoal.h"
 
 #include <message_filters/subscriber.h>
 #include <message_filters/synchronizer.h>
@@ -93,7 +94,10 @@ private:
 						   const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
 						   const sensor_msgs::LaserScanConstPtr& scanMsg,
 						   const nav_msgs::OdometryConstPtr & odomMsg);
-	void goalNodeCallback(const std_msgs::Int32ConstPtr & msg);
+
+	void goalCommonCallback(const std::list<std::pair<int, rtabmap::Transform> > & poses);
+	void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
+	void goalGlobalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
 	void updateGoal(const ros::Time & stamp);
 
 	void process(
@@ -119,6 +123,7 @@ private:
 	bool setModeMappingCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& rep);
 	bool publishMapCallback(rtabmap_ros::PublishMap::Request&, rtabmap_ros::PublishMap::Response&);
+	bool setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res);
 
 	rtabmap::ParametersMap loadParameters(const std::string & configFile);
 	void saveParameters(const std::string & configFile);
@@ -126,7 +131,7 @@ private:
 	void publishLoop(double tfDelay);
 
 	void publishStats(const rtabmap::Statistics & stats, const ros::Time & stamp);
-	void publishGoal(const ros::Time & stamp);
+	void publishCurrentGoal(const ros::Time & stamp);
 	void publishLocalPath(const ros::Time & stamp);
 
 private:
@@ -151,7 +156,8 @@ private:
 	ros::Publisher mapGraph_;
 
 	//Planning stuff
-	ros::Subscriber goalNodeSub_;
+	ros::Subscriber goalSub_;
+	ros::Subscriber goalGlobalSub_;
 	ros::Publisher nextMetricGoalPub_;
 	ros::Publisher goalReachedPub_;
 	ros::Publisher globalPathPub_;
@@ -226,6 +232,7 @@ private:
 	ros::ServiceServer setModeMappingSrv_;
 	ros::ServiceServer getMapDataSrv_;
 	ros::ServiceServer publishMapDataSrv_;
+	ros::ServiceServer setGoalSrv_;
 
 	boost::thread* transformThread_;
 

src/GridMapAssemblerNode.cpp

@@ -95,7 +95,7 @@ public:
 
 		if(filterRadius_ > 0.0 && filterAngle_ > 0.0)
 		{
-			poses = rtabmap::radiusPosesFiltering(poses, filterRadius_, filterAngle_*CV_PI/180.0);
+			poses = rtabmap::graph::radiusPosesFiltering(poses, filterRadius_, filterAngle_*CV_PI/180.0);
 		}
 
 		if(gridMap_.getNumSubscribers())

src/MapAssemblerNode.cpp

@@ -209,7 +209,7 @@ public:
 		}
 		if(nodeFilteringAngle_ > 0.0 && nodeFilteringRadius_ > 0.0)
 		{
-			poses = rtabmap::radiusPosesFiltering(poses, nodeFilteringRadius_, nodeFilteringAngle_*CV_PI/180.0);
+			poses = rtabmap::graph::radiusPosesFiltering(poses, nodeFilteringRadius_, nodeFilteringAngle_*CV_PI/180.0);
 		}
 
 		if(assembledMapClouds_.getNumSubscribers())

src/MapOptimizerNode.cpp

@@ -212,12 +212,12 @@ public:
 			{
 				if(optimizeFromLastNode_)
 				{
-					std::map<int, int> depthGraph = rtabmap::generateDepthGraph(constraints, poses.rbegin()->first);
+					std::map<int, int> depthGraph = rtabmap::graph::generateDepthGraph(constraints, poses.rbegin()->first);
-					rtabmap::optimizeTOROGraph(depthGraph, poses, constraints, optimizedPoses, iterations_, true, ignoreVariance_);
+					rtabmap::graph::optimizeTOROGraph(depthGraph, poses, constraints, optimizedPoses, iterations_, true, ignoreVariance_);
 				}
 				else
 				{
-					rtabmap::optimizeTOROGraph(poses, constraints, optimizedPoses, iterations_, true, ignoreVariance_);
+					rtabmap::graph::optimizeTOROGraph(poses, constraints, optimizedPoses, iterations_, true, ignoreVariance_);
 				}
 
 				mapToOdomMutex_.lock();

src/rviz/MapCloudDisplay.cpp

@@ -325,7 +325,7 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
 
 	if(node_filtering_angle_->getFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f)
 	{
-		poses = rtabmap::radiusPosesFiltering(poses,
+		poses = rtabmap::graph::radiusPosesFiltering(poses,
 				node_filtering_radius_->getFloat(),
 				node_filtering_angle_->getFloat()*CV_PI/180.0);
 	}

srv/SetGoal.srv

@@ -0,0 +1,5 @@
+#request
+int32 target_node_id
+bool in_global_graph
+---
+#response