Sending Goals with RTABMAP and move_base

[aibrain]

Hi,

I'm attempting to put a navigation stack in place for the first time. Being new to this, I have barely tuned most parameters, relying instead on default settings from online tutorials etc.

Recap of my steps:

1) I mapped my office, created a database, and launched Rtabmapviz in localization mode. The localization seems to be working.



My launch file is included here for reference (I'm not using RVIZ even though the filename has that): rtabmap_rviz.launch

2) As the launch file indicates, I'm launching move_base customized with the following parameters:
base_local_planner_params.yaml
costmap_common_params.yaml
local_costmap_params.yaml
global_costmap_params.yaml

move_base launches and gets to this point:



3) Now that move base is running, and Rtabmapviz is running in localization, I find the ID of a destination node by hovering the mouse over it. Then I do Detection->Send Goal and pass the destination.

However the robot remains still, and nothing happens. Reviewing the Rtabmap output I see the following:

[ INFO] [1472236190.849796586]: rtabmap: Rate=1.00s, Limit=0.000s, RTAB-Map=0.0519s, Maps update=0.0070s pub=0.0055s (local map=598, WM=598)
[ INFO] [1472236192.009414060]: Publishing next goal: 1901 -> xyz=2.384681,0.101888,-0.135162 rpy=-0.001140,-0.011145,0.443476
[ INFO] [1472236192.009532532]: rtabmap: Rate=1.00s, Limit=0.000s, RTAB-Map=0.0752s, Maps update=0.0086s pub=0.0078s (local map=598, WM=598)
[ INFO] [1472236193.090997165]: Publishing next goal: 1901 -> xyz=2.384681,0.101888,-0.135162 rpy=-0.001140,-0.011145,0.443476
[ INFO] [1472236193.091119626]: rtabmap: Rate=1.00s, Limit=0.000s, RTAB-Map=0.0612s, Maps update=0.0059s pub=0.0059s (local map=598, WM=598)
[ INFO] [1472236194.191680135]: Publishing next goal: 1901 -> xyz=2.384681,0.101888,-0.135162 rpy=-0.001140,-0.011145,0.443476
[ INFO] [1472236194.191806155]: rtabmap: Rate=1.00s, Limit=0.000s, RTAB-Map=0.0611s, Maps update=0.0075s pub=0.0071s (local map=598, WM=598)
[ INFO] [1472236195.263901258]: Publishing next goal: 1901 -> xyz=2.384681,0.101888,-0.135162 rpy=-0.001140,-0.011145,0.443476
[ INFO] [1472236195.264066398]: rtabmap: Rate=1.00s, Limit=0.000s, RTAB-Map=0.0507s, Maps update=0.0083s pub=0.0056s (local map=598, WM=598)
[ INFO] [1472236196.357349658]: Publishing next goal: 1901 -> xyz=2.384681,0.101888,-0.135162 rpy=-0.001140,-0.011145,0.443476
[ INFO] [1472236196.357418809]: rtabmap: Rate=1.00s, Limit=0.000s, RTAB-Map=0.0919s, Maps update=0.0004s pub=0.0012s (local map=598, WM=598)

I also attempted labeling individual positions within the map and sending goals as labels, with no actionable result.

I figure the problem is either with the the setup of Rtabmap or move_base. Could these configurations help identify what the issue is?

Thanks

[matlabbe]

Hi,

the goals are sent on /rtabmap/goal_out topic. Either remap this topic to "/move_base_simple/goal" (input of move_base) or set "use_action_for_goal" to true in rtabmap.launch (under rtabmap node). The later will connect to move_base actions interface, so it will have feedback if move_base fails executing a goal.

For convenience, note that you can send also poses as goals with RVIZ to rtabmap with the "2D nav goal" tool, just set /rtabmap/goal as the topic to send them. You can also send goals directly to move_base on "/move_base_simple/goal" topic.

cheers

[aibrain]

Thanks, inline with your suggestion I modified the rtabmap.launch file (unter the rtabmap node) to set :

param name="use_action_for_goal" type="bool" value="true"

However now when I attempt to send a location id as a goal, the following error appears in the console:

[ERROR] [1472491854.402744004]: Planning: Node id should be > 0 !

That seems strange since the numerical id I sent was definitely positive.

[matlabbe]

Do you have this warning just over the error?

[ WARN] (2016-08-29 14:42:41.777) Rtabmap.cpp:3455::computePath() Working memory is empty... cannot compute a path
[ERROR] [1472496161.777620641]: Planning: Node id should be > 0 !

Well, the error msg is wrong. I'll check that. To correctly plan, there should be a map. EDIT and the goal is actually in the graph.

[aibrain]

Yes, that is the warning I see, preceding the error.

How to resolve this?

I only send the goal after downloading all the point clouds. As a sanity check I run "tf view_frames" and confirm that /map is in the TF tree. Shouldn't that mean we have a map?

I am mousing over nodes in the graph display and sending the first integer displayed as a goal. Is that the recommended workflow? Shouldn't that mean the goal is in the graph?

Thanks!

[aibrain]

I think the issue was that I was sending the goal before it had been localized. Instead I waited for a loop closure before sending the goal.

However now, I get a steady stream of "Cannot connect to move_base action server" messages, despite ROS_MASTER_URI and ROS_IP having been set. I'm not sure why ....

[aibrain]

The message seems to originate here:

https://github.com/introlab/rtabmap_ros/blob/master/src/CoreWrapper.cpp

Would you have any guess as to what I might be doing to cause it?

Thanks.

[matlabbe]

Hi,

The error says that it cannot connect to move_base. Either move_base is not running or it is in a different namespace or have a different name.

The move_base action client is created here:

mbClient_("move_base", true)

You may have to remap "move_base" to something else if on your system it has a different name or is in a different namespace:

<group ns="rtabmap">
   <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap">
      [...]
      <remap from="move_base" to="/move_base"/>
   </node>
</group>

In this example, rtabmap node is created in rtabmap namespace, to the default name is "/rtabmap/move_base". It is renamed to public namespace instead: "/move_base".

cheers

[aibrain]

Thanks for your help. After re-mapping, move_base now receives the goals.

My new issue is that the goal poses seem to specify orientations in which the z axis isn't strictly vertical. Since move_base doesn't expect that, it complains.

ROS_ERROR("Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical.");

Is there a way to map so that the critical points are planar? I was considering remapping with depthimage_to_laserscan information, thinking that this may improve the robustness of the proj_map.

[matlabbe]

The quaternion of your goals may not be normalized. Are you sending them using RVIZ tool or by terminal?

If you use depthimage_to_laserscan, you would have to set for rtabmap node "subscribe_scan" to true and remap "/scan" topic to yours. This will produce grid_map. With the laser scan, you may not detect obstacles under/over it.

cheers

[felipeduque]

(Bumping this old thread for I'm facing the same issue)

Sometimes "move_base" complains that the goal's quaternion is invalid, saying that it should be close to vertical. However, it happens only sometimes (lately, more often than not), and I'm sending the goals via rtabmapviz - I'm sending the node ID as the goal. What could be the problem?

Edit: I've just noticed that it only happens for certain nodes, e.g., if I set node 131 as the goal, the robot moves accordingly. If I set node 120, the quaternion error comes up.

[matlabbe]

Can you do a rostopic echo of the goal topic sent to move_base when the error happens?

Also attach the graph to this post:

$ rostopic echo /rtabmap/mapGraph

A fix could be to normalize the quaternion before sending it to make sure it is valid. The transform taken from rtabmap is converted to a pose message here before being published. Inside that function, we could do like in that function and normalize the quaternion. If this fixes the problem, I will make the change.

cheers,
Mathieu