Add function to set the initial pose from RVIZ for pure localization mode #1284

tongtybj · 2019-05-12T10:34:16Z

As mentioned in the closed PR #637, manually "tuning" of the robot initial pose from GUI (e.g. Rviz) is a very important function for home service robot, which is supported by the ROS default localization method AMCL.

I hope this PR can facilitate the development of this RFC.

To test my commit, please try following commands

First follow the tutorial about pure localization to download rosbags and build the map (.pbstream).
Then run following command instead:

$ roslaunch cartographer_ros demo_backpack_2d_localization.launch \
   load_state_filename:=${HOME}/Downloads/b3-2016-04-05-13-54-42.bag.pbstream \
   rosbag:=false

Use the icon 2D Pose Estimation in Rviz to set the initial pose as shown in following image.
Play the rosbag:

$ rosbag play ${HOME}/Downloads/b2-2016-04-27-12-31-41.bag --clock -s 320

The whole sequence can be found in this video, and you can also try the 3D mode in this same way.

We have confirmed the 3D performance with our original data, please check this video.

Limitations: the multi-floor map is not supported, since we can not assign the height (i.e. z value) from Rviz.

…mode

faizansid77 · 2019-05-17T07:32:51Z

I got the following error while I built your code

FAILED: /usr/bin/c++   -DROSCONSOLE_BACKEND_LOG4CXX -DROS_PACKAGE_NAME=\"cartographer_ros\" -I/home/faizansid/catkin_carto/src/cartographer_ros/cartographer_ros/cartographer_ros/cartographer_ros -isystem /usr/include/lua5.2 -isystem /usr/include/pcl-1.7 -isystem /usr/include/eigen3 -isystem /home/faizansid/catkin_carto/install_isolated/include -isystem /opt/ros/kinetic/include -isystem /opt/ros/kinetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -I. -I/home/faizansid/catkin_carto/src/cartographer_ros/cartographer_ros -isystem /usr/include/cairo -isystem /usr/include/glib-2.0 -isystem /usr/lib/x86_64-linux-gnu/glib-2.0/include -isystem /usr/include/pixman-1 -isystem /usr/include/freetype2 -isystem /usr/include/libpng12 -isystem /usr/local/include -O3 -DNDEBUG    -pthread -std=c++11 -fPIC  -Wall -Wpedantic -Werror=format-security -Werror=missing-braces -Werror=reorder -Werror=return-type -Werror=switch -Werror=uninitialized -O3 -DNDEBUG -MMD -MT cartographer_ros/CMakeFiles/set_initpose_from_rviz.dir/set_initpose_main.cc.o -MF cartographer_ros/CMakeFiles/set_initpose_from_rviz.dir/set_initpose_main.cc.o.d -o cartographer_ros/CMakeFiles/set_initpose_from_rviz.dir/set_initpose_main.cc.o -c /home/faizansid/catkin_carto/src/cartographer_ros/cartographer_ros/cartographer_ros/set_initpose_main.cc
/home/faizansid/catkin_carto/src/cartographer_ros/cartographer_ros/cartographer_ros/set_initpose_main.cc: In function ‘void move_base_simple_callback(const ConstPtr&)’:
/home/faizansid/catkin_carto/src/cartographer_ros/cartographer_ros/cartographer_ros/set_initpose_main.cc:123:32: error: ‘cartographer_ros_msgs::StartTrajectory::Request {aka struct cartographer_ros_msgs::StartTrajectoryRequest_<std::allocator<void> >}’ has no member named ‘configuration_directory’
   srv_start_trajectory.request.configuration_directory =
                                ^
/home/faizansid/catkin_carto/src/cartographer_ros/cartographer_ros/cartographer_ros/set_initpose_main.cc:125:32: error: ‘cartographer_ros_msgs::StartTrajectory::Request {aka struct cartographer_ros_msgs::StartTrajectoryRequest_<std::allocator<void> >}’ has no member named ‘configuration_basename’
   srv_start_trajectory.request.configuration_basename =
                                ^
/home/faizansid/catkin_carto/src/cartographer_ros/cartographer_ros/cartographer_ros/set_initpose_main.cc:128:32: error: ‘cartographer_ros_msgs::StartTrajectory::Request {aka struct cartographer_ros_msgs::StartTrajectoryRequest_<std::allocator<void> >}’ has no member named ‘relative_to_trajectory_id’
   srv_start_trajectory.request.relative_to_trajectory_id =
                                ^
/home/faizansid/catkin_carto/src/cartographer_ros/cartographer_ros/cartographer_ros/set_initpose_main.cc:130:32: error: ‘cartographer_ros_msgs::StartTrajectory::Request {aka struct cartographer_ros_msgs::StartTrajectoryRequest_<std::allocator<void> >}’ has no member named ‘use_initial_pose’
   srv_start_trajectory.request.use_initial_pose = true;
                                ^
/home/faizansid/catkin_carto/src/cartographer_ros/cartographer_ros/cartographer_ros/set_initpose_main.cc:131:65: error: ‘cartographer_ros_msgs::StartTrajectory::Request {aka struct cartographer_ros_msgs::StartTrajectoryRequest_<std::allocator<void> >}’ has no member named ‘initial_pose’
   tf2::toMsg(relative_initpose_tf, srv_start_trajectory.request.initial_pose);
                                                                 ^
/home/faizansid/catkin_carto/src/cartographer_ros/cartographer_ros/cartographer_ros/set_initpose_main.cc: In function ‘int main(int, char**)’:
/home/faizansid/catkin_carto/src/cartographer_ros/cartographer_ros/cartographer_ros/set_initpose_main.cc:169:18: error: ‘absl’ has not been declared
   map_builder_ = absl::make_unique<cartographer::mapping::MapBuilder>(
                  ^
/home/faizansid/catkin_carto/src/cartographer_ros/cartographer_ros/cartographer_ros/set_initpose_main.cc:169:69: error: expected primary-expression before ‘>’ token
   map_builder_ = absl::make_unique<cartographer::mapping::MapBuilder>(
                                                                     ^

tongtybj · 2019-05-18T04:10:25Z

@faizansid77

"cartographer_ros_msgs::StartTrajectory" has been changed by #1245 , and absl::make_unique is introduced by #955 .

Can you tell me the detail about the build condition?

MichaelGrupp

Although I can see that this code is useful for your type of experiments, I don't think this code serves the general use case as it makes a lot of assumptions... For example, a pose sent on the /initialpose topic is not necessarily pure 2D and 3D users probably don't always want the z-component to be automatically set.

But I fear that initial pose handling is something that almost always requires some customization, so I don't think we will find an implementation that is generally useful so it can be integrated into this repository... especially one that supports both 2D and 3D.

P.S.: But maybe it makes sense to help people with implementing their custom initial pose tool by providing a minimal example, e.g. in the documentation?
Because a minimal initial pose handler can be implemented with a few lines of Python or C++ code using the ROS services /get_trajectory_states, /finish_trajectory and /start_trajectory.

MichaelGrupp · 2019-05-20T08:20:36Z

cartographer_ros/cartographer_ros/set_initpose_main.cc

+              "Filename of a pbstream to draw a map from.");
+
+namespace {
+int current_trajectory_id_ = 1;


This is a hack, and there are numerous possible situations where this would be wrong.

A proper implementation would be to check the trajectory states for an active trajectory. The ROS service for that is /get_trajectory_states.

Thank you for your advice. I have revised based on your comment (f0b35a4)

MichaelGrupp · 2019-05-20T08:25:33Z

cartographer_ros/cartographer_ros/set_initpose_main.cc

+      FLAGS_configuration_directory, FLAGS_configuration_basename);
+  map_builder_ = absl::make_unique<cartographer::mapping::MapBuilder>(
+      node_options.map_builder_options);
+  map_builder_->LoadState(&reader, true);


Why do you have a map builder? It's not used.

map build is used to get the frozen trajectory information:
https://github.com/googlecartographer/cartographer_ros/blob/5b09b08daf6a6f1f51296f44e11f8c1f5ba48d40/cartographer_ros/cartographer_ros/set_initpose_main.cc#L109

The trajectory information is required here, since the initial pose of the new trajectory should be provided w.r.t the reference trajectory (i.e. the frozen trajectory). Is it right?

MichaelGrupp · 2019-05-20T08:32:26Z

cartographer_ros/cartographer_ros/set_initpose_main.cc

+
+    // find the best submap, which is closest to the initial pose
+    // and assign the height (i.e. z value) of this submap to the initial pose
+    if (delta_pos.length() < min_dist) {


oof... this looks like it could have funny side effects in buildings with multiple floors.

I have removed this part (5b09b08)

…he rosserical "/get_trajectory_states"

tongtybj · 2019-06-11T13:27:50Z

@MichaelGrupp Thank you for your review. I agree that my original implementation is specialized for the 2.5D case. So I improve the code according to your comments.

But, about following comment, I want to discuss further:

Because a minimal initial pose handler can be implemented with a few lines of Python or C++ code using the ROS services /get_trajectory_states, /finish_trajectory and /start_trajectory.

Besides these three services, I think the information about the reference trajectory (i.e. the frozen trajectory) is also necessary, since the initial pose should be provided w.r.t. the reference trajectory; otherwise, users should covert the transformation by themselves (but common robotics users are tend to use simple gui like rviz):
https://github.com/googlecartographer/cartographer_ros/blob/5b09b08daf6a6f1f51296f44e11f8c1f5ba48d40/cartographer_ros/cartographer_ros/set_initpose_main.cc#L109-L124

BTW, calling /finish_trajectory can stop an ongoing trajectory building, but the map would remain. This effects the navigation performance (e.g. move_base) based on the /map topic published by cartographer_occupancy_grid_node. So I think there should be a service to delete the unnecessary trajectory, or is there already some solution?

MUZUIXIAOHAI

20c2c8f

wesigj · 2020-11-05T09:14:17Z

@tongtybj
have you finish this issue, i want to use 'Add function to set the initial pose from RVIZ for pure localization mode ' this function in my cartographer,but i don't know how to use your function, could you help me?
Thank you for your contributions

greymfm · 2021-07-04T22:59:05Z

Maybe it's more elegant to do the rviz initial pose feature 'outside' Cartographer via a simple Python script? At least that's how I did it :-) #1652

Add function to set the initial pose from RVIZ for pure localization …

d23f2ec

…mode

googlebot added the cla: yes label May 12, 2019

pifon2a and others added 2 commits May 12, 2019 12:38

Ran clang-format.

553b59d

Add dependency tf2_geometry_msgs

b26fcd5

MichaelGrupp reviewed May 20, 2019

View reviewed changes

tongtybj added 2 commits June 11, 2019 21:27

Stop the active trajectory before reseting the initpose, by calling t…

f0b35a4

…he rosserical "/get_trajectory_states"

Remove the heuristic part to assign the z position for initial pose

5b09b08

Ran clang-format.

65d0714

MUZUIXIAOHAI reviewed Aug 20, 2019

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add function to set the initial pose from RVIZ for pure localization mode #1284

Add function to set the initial pose from RVIZ for pure localization mode #1284

tongtybj commented May 12, 2019

faizansid77 commented May 17, 2019

tongtybj commented May 18, 2019

MichaelGrupp left a comment

MichaelGrupp May 20, 2019

tongtybj Jun 11, 2019 •

edited

Loading

MichaelGrupp May 20, 2019

tongtybj Jun 11, 2019

MichaelGrupp May 20, 2019

tongtybj Jun 11, 2019

tongtybj commented Jun 11, 2019 •

edited

Loading

MUZUIXIAOHAI left a comment

wesigj commented Nov 5, 2020

greymfm commented Jul 4, 2021

Add function to set the initial pose from RVIZ for pure localization mode #1284

Are you sure you want to change the base?

Add function to set the initial pose from RVIZ for pure localization mode #1284

Conversation

tongtybj commented May 12, 2019

faizansid77 commented May 17, 2019

tongtybj commented May 18, 2019

MichaelGrupp left a comment

Choose a reason for hiding this comment

MichaelGrupp May 20, 2019

Choose a reason for hiding this comment

tongtybj Jun 11, 2019 • edited Loading

Choose a reason for hiding this comment

MichaelGrupp May 20, 2019

Choose a reason for hiding this comment

tongtybj Jun 11, 2019

Choose a reason for hiding this comment

MichaelGrupp May 20, 2019

Choose a reason for hiding this comment

tongtybj Jun 11, 2019

Choose a reason for hiding this comment

tongtybj commented Jun 11, 2019 • edited Loading

MUZUIXIAOHAI left a comment

Choose a reason for hiding this comment

wesigj commented Nov 5, 2020

greymfm commented Jul 4, 2021

tongtybj Jun 11, 2019 •

edited

Loading

tongtybj commented Jun 11, 2019 •

edited

Loading