Added Tf publisher #30

vikcp · 2019-06-07T10:33:06Z

Related to issue #29

I get an error "bad alloc" from the grid_compositor.cpp at line 66 when it tries to resize the "result_grid".

Thanks.

hrnr

See below a possible reason for your crashes. As always with multi-threaded code I would advise you to run it with thread sanitizer to check possible problems.

hrnr · 2019-06-13T18:04:12Z

map_merge/src/map_merge.cpp

@@ -60,11 +60,14 @@ MapMerge::MapMerge() : subscriptions_size_(0)
                                robot_map_updates_topic_, "map_updates");
  private_nh.param<std::string>("robot_namespace", robot_namespace_, "");
  private_nh.param<std::string>("merged_map_topic", merged_map_topic, "map");
-  private_nh.param<std::string>("world_frame", world_frame_, "world");
+  private_nh.param<std::string>("world_frame", world_frame_, "map");


is this intended?

Yes, but it is because in my opinion each robot map should have the namespace of the robot, and then, the global map, just be called map.
But it is just how I imagine it. I can revert this change before the pull request.

hrnr · 2019-06-13T18:06:52Z

map_merge/src/map_merge.cpp

+{
+  if (!tf_current_flag_.test_and_set()) {
+	// need to recalculate stored transforms
+	auto transforms = pipeline_.getTransforms();  // this is thread-safe access


this is not a thread-safe access AFAIK

It think this may be the cause of the crash you experienced.

Ok! I'll have a look to this.

I have checked with TSan, and as you said, there was an issue there.
I have added a mutex to protect it.
With TSan I still have other warnings, however, I can't find the issue and I haven't experienced any other crashes.

hrnr · 2019-06-13T18:07:29Z

map_merge/src/map_merge.cpp

@@ -387,6 +433,15 @@ void MapMerge::spin()
  std::thread merging_thr([this]() { executemapMerging(); });
  std::thread subscribing_thr([this]() { executetopicSubscribing(); });
  std::thread estimation_thr([this]() { executeposeEstimation(); });
+  if (publish_tf) {


could we make this similar to those above?

Done, I should have done it your way since the beginning.

hrnr · 2019-06-13T19:54:26Z

map_merge/src/map_merge.cpp

+  }
+}
+
+std::vector<geometry_msgs::TransformStamped> MapMerge::StampTransforms(const std::vector<geometry_msgs::Transform> transforms_, const std::string& frame)


functions start with lowercase letter. this function should be static inline non-member or static member as it does not access any member variables. names with trailing underscore indicate member variables and should not be used as arguments.

This function actually uses two member variables, which allow proper map naming. I changed the name of the transforms parameter not to confuse.
So should I leave this function as a member function as it uses two member variables or should I keep it static and pass the variables as parameters?

…efinition as the previous.

cero10 · 2019-08-05T16:31:34Z

Is there a way I can get a version of the map merger where I can freely control the merged map's origin, by changing the initial poses of the robots? because I tried this branch but the merged map still has an offset.

vikcp · 2019-08-06T07:49:16Z

Is there a way I can get a version of the map merger where I can freely control the merged map's origin, by changing the initial poses of the robots? because I tried this branch but the merged map still has an offset.

This PR does not involve the initial poses of the robot nor the creation of the merged map. It is just to publish the transforms between maps.

andresjguevara · 2019-11-08T18:21:55Z

Is this feature ready to be merged?

hrnr · 2019-11-10T19:23:20Z

@andresjguevara It would need couple of adjustments. Would you be interested to contribute those?

vikcp · 2019-11-11T15:43:19Z

Hi,
I have been working on this feature for a while.
I finally find out how to get these transforms, because what I found was that OpenCV provides the proper rotation, but not the translation, so you need to manually reverse the process to obtain the translation.
I need to clean up the code, and then I can PR again to see if it fits here.
I'd need a couple of days

vikcp · 2019-11-11T16:45:39Z

I have updated the PR with my last modifications.

@hrnr I have commented the "adjuster" because it was misleading the results. I don't know if you have other experience with this but it helped a lot no to add it in the loop.

@andresjguevara If the PR takes time or does not fit, you can use my fork of this work and try my branch called publish transforms, it should be working. However, take into account that no transform is given if the maps do not stitch and if the stitching is wrong it will provide wrong transforms.

hrnr · 2019-11-11T20:42:03Z

Thanks, I can take a look on Wednesday. I'm planing to release for Melodic, so it would be great to get your changes in.

Xvdgeest · 2019-11-27T15:19:04Z

This looks awesome, exactly what I need.
When will this feature be merged? Will it also be for kinetic?
Is there still help needed?

Xvdgeest · 2019-11-28T12:34:10Z

Thank you guys very much for putting the work in to get the transforms published. I cloned this branch and ran it on two mobile robots using rplidar and gmapping to create the (same) map. The robots are moving in the same area, thus seeing each other, which causes some disturbance.

I noticed that the transforms are very "jumpy". When the maps are merged (almost) correctly, The rotations align but the translation is off by a few meters. Also, the maps are drawn at (x,y)=(100,100), which is not a problem but it is a bit strange.

I'll see if I can find a solution, if you have any ideas, help would be greatly appreciated.

vikcp · 2019-11-28T14:21:25Z

@Xvdgeest
Hi,
Regarding the "jumpy" transforms it might be cause for several reasons. One of them it could be due to the rate in which maps are updated and its new information processed. Are these transforms more stable once the map is closer to a completion and it does not expands anymore?
Regarding the initial position, the merged map is set initially at (0,0) but the maps that you are creating with gmapping can be initiated at a different position depending on the parameters that you use or your odometry values.
Please report any issues that you find so that we can all try to improve this feature.
Thank you!

@hrnr
Hi! Did you have time to check this PR?
It will be very helpful if you could have a look at the code.
Thank you!

jkRaccoon · 2021-09-29T08:31:22Z

@hrnr hi~! I`m still wait for this pr :)

tristan-schwoerer · 2021-11-22T17:55:14Z

@hrnr Is this viable to use? Would be amazing if we can get this PR approved then

Added Tf publisher in a thread and corrected transform initialization.

91dcbc7

hrnr reviewed Jun 13, 2019

View reviewed changes

Issues from PR solved. Added mutex, changed function and new thread d…

a0a3e4d

…efinition as the previous.

vikcp added 5 commits August 6, 2019 16:24

Added debugging tools

901f7a4

before refactoring

076117f

logs and refactoring of merging_pipeline

257a5cf

Before not needing orgin at (0,0)

586d9e0

no need to modify map origin

31c2585

clean few last ros_debugs

d4d548d

Xvdgeest mentioned this pull request Dec 3, 2019

Using laser_scan_matcher to find transforms between 2 robots with lidar. CCNYRoboticsLab/scan_tools#64

Open

hrnr mentioned this pull request Jan 3, 2021

how to get the trasformation to map merger #43

Closed

Neidr4 mentioned this pull request May 20, 2021

merged_map origin Neidr4/ptdr#8

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Added Tf publisher #30

Added Tf publisher #30

vikcp commented Jun 7, 2019

hrnr left a comment

hrnr Jun 13, 2019

vikcp Jun 14, 2019

hrnr Jun 13, 2019

hrnr Jun 13, 2019

vikcp Jun 14, 2019

vikcp Jul 29, 2019

hrnr Jun 13, 2019

vikcp Jul 29, 2019

hrnr Jun 13, 2019

vikcp Jul 29, 2019

cero10 commented Aug 5, 2019 •

edited

Loading

vikcp commented Aug 6, 2019

andresjguevara commented Nov 8, 2019

hrnr commented Nov 10, 2019

vikcp commented Nov 11, 2019

vikcp commented Nov 11, 2019

hrnr commented Nov 11, 2019

Xvdgeest commented Nov 27, 2019

Xvdgeest commented Nov 28, 2019

vikcp commented Nov 28, 2019

jkRaccoon commented Sep 29, 2021

tristan-schwoerer commented Nov 22, 2021

Added Tf publisher #30

Are you sure you want to change the base?

Added Tf publisher #30

Conversation

vikcp commented Jun 7, 2019

hrnr left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

cero10 commented Aug 5, 2019 • edited Loading

vikcp commented Aug 6, 2019

andresjguevara commented Nov 8, 2019

hrnr commented Nov 10, 2019

vikcp commented Nov 11, 2019

vikcp commented Nov 11, 2019

hrnr commented Nov 11, 2019

Xvdgeest commented Nov 27, 2019

Xvdgeest commented Nov 28, 2019

vikcp commented Nov 28, 2019

jkRaccoon commented Sep 29, 2021

tristan-schwoerer commented Nov 22, 2021

cero10 commented Aug 5, 2019 •

edited

Loading