Cartographer_ros: set starting point of trajectory & reload new map for pure localization

[cgdsss]

Hi, I want to achieve two goals by modifying the pure localization mode:

• set the starting point in the map coordinate system of the new trajectory.
• Reload new .pbstream map, overwrite old map.
  1.　part of my code is as follows:

    else if(PEEK_CMD_FF(cmd, "initpose", 8, f1, f2))
    {
        Eigen::Matrix<double, 3, 1> position(f1, f2, 0);
        Eigen::Quaternion<double> rotation(1, 0, 0, 0);
        ::cartographer::mapping::proto::InitialTrajectoryPose pose;
        pose.set_to_trajectory_id(0);
        *pose.mutable_relative_pose() =
            cartographer::transform::ToProto(cartographer::transform::Rigid3d(position, rotation));
        pose.set_timestamp(cartographer::common::ToUniversal(FromRos(ros::Time::now())));
        *last_trajectory_options_.trajectory_builder_options.mutable_initial_trajectory_pose() = pose;
        StartTrajectoryWithDefaultTopics(last_trajectory_options_);
        ROS_INFO("start initpose trajectory");
    }

But I found that the initial coordinates of the settings seem to be based on the odom coordinate system. How to set the coordinates based on map frame?

1. for second purpose, my code as follows:

 else if(PEEK_CMD_S(cmd, "loadmap", 7, str1))
    {
        LoadState(str1, true);
        ROS_INFO("load: %s", str1);
    }

But calling this function twice will load two maps at the same time, how to clear the previous map and import a new map for pure localization?

[zhengzh]

1. There are two options to reload a new map, (a) delete the Node object in node_main.cc and create a new Node with new map. (b) or use python subprocess to relaunch cartographer_node program, the code looks like this

#!/usr/bin/env python

import rospy
import subprocess
import signal

child = subprocess.Popen(["roslaunch","cartographer_ros",
"demo_backpack_2d_localization_start_trajectory.launch",
"load_state_filename:=b2-2016-04-05-14-44-52.bag.pbstream",
"bag_filename:=b2-2016-04-27-12-31-41.bag"])
# child.wait() #You can use this line to block the parent process untill the child process finished.

print("parent process")
print(child.poll())

rospy.loginfo('The PID of child: %d', child.pid)
print ("The PID of child:", child.pid)

rospy.sleep(5)

# child.send_signal(signal.SIGINT) #You may also use .terminate() method
# child.terminate()

child2 = subprocess.Popen(["roslaunch","cartographer_ros",
"start_trajectory.launch",])
child.wait()

#for more: https://docs.python.org/2/library/subprocess.html

1. To start a new trajectory with initial pose

rosrun cartographer_ros cartographer_start_trajectory \ 
-configuration_directory="${CONFIGURATION_DIRECTORY}" \
-configuration_basename="${CONFIGURATION_BASENAME}"  \
--initial_pose="{ to_trajectory_id=0, relative_pose={translation={10, 10, 0},rotation={0.0,0.0,0.0} }}"

[edward0im]

I've modified start_trajectory_main.cc codes to change starting point at the initial time.

and It worked almost normally (heading of clicked point is not correct, position of clicked point is not correct as well).
but pure localization has failed for my case.

I upload this modified codes and hope it helps for you :-)

localization_test.launch
https://gist.github.com/tigerk0430/aee06ddb04c2632c92917d5343e3ba36

start_trajectory_main.cc
https://gist.github.com/tigerk0430/7f67a9ca10824ea56875ad4b7a449b49

init_pose_test.launch
https://gist.github.com/tigerk0430/6ecd59f2f7905f10edc2ec7b93e27264

Instruction

1. roslaunch cartographer_ros localization_test.launch load_state_filename:=.../.pbstream
2. rosbag play --clock ....bag
3. roslaunch init_pose_test.launch
4. Rviz, click 2D Nav Goal and click a position you want to

[carl1218]

@tigerk0430
Hi,
I am running 2d slam,i want to set the new trajectory in the new plane(Z-axis ≠ 0),and save the map.how can i do this?
I call rosservice call /finish_trajectory,and set the new trajectory to z-axis=5,i runned:
rosrun cartographer_ros cartographer_start_trajectory -configuration_basename=backpack_2d.lua -configuration_directory=/home/calyu/catkin_ws/install_isolated/share/cartographer_ros/configuration_files -initial_pose="{to_trajectory_id=0,relative_pose={translation={0.0,0.0,5},rotation={0.,0.,0.0}}}
i can see the map look like this:

i want to save this map ,so than i runned rosservice call /finish_trajectory 1,but the Z axis will change to the initial position.(Z-axis = 0m).

How can i to set the new trajectory in the new plane (Z-axis ≠ 0) and save this map? Can you help me.
thank you.

[edward0im]

Hello @carl1218,

It might be the problem of cartographer_start_trajectory

rosrun cartographer_ros cartographer_start_trajectory {...} -initial_pose="{to_trajectory_id=0,relative_pose={translation={0.0,0.0,5},rotation={0.,0.,0.0}}}

It generates new trajectory when you run this code. but relative pose is generated from end of the to_trajectory_id's node's pose.
It isn't generated from the start of the to_trajectory_id's node's pose. that's kind of confusing.

for example, when to_trajectory_id = 0, you should get the end of Trajectory 0's pose to use relative pose.
if you still have same problem. let me know with more detail

Thank you!

[carl1218]

@tigerk0430
Hi,
Thank you very much for your reply.
I want to have multiple floors with 2D SLAM, and use separate 2D maps per floor.
I want just process and store the data separately and use separate Cartographer nodes for each floor.

I want to have multiple 2D maps aligned with a z-offset in 3D space, i need an additional coordinate frame to model the floors.maybe i need define an additional parent TF frame (let's call it "world"), in which the floor maps are located. The static transformation from "world" to each map can be adjusted to model the floor height.
Because I don't know much about the cartographer, in order to achieve the effect I need, can you help me modify it, of course, it's not free.
Expected for your reply.

[edward0im]

hello @carl1218

I understand what you're currently working on, but I'm now too busy to cooperate with you ;-/
I think the best way to achieve your goal, email to cartographer developers whenever you've got some problem.
here is a public space and they don't have enough time to give an answer to every person.

have a good day :-)

[shaoxiang]

@cgdsss
Hi, Do you have a good solution to the problem reload maps? I don't want to use the way of roslaunch.
I also want to load maps dynamically. Thanks.

[MichaelGrupp]

The /start_trajectory ROS service was simplified recently and should be easier to use now. See #1245