search

SteveMacenski / slam_toolbox

Slam Toolbox for lifelong mapping and localization in potentially massive maps with ROS
GNU Lesser General Public License v2.1
1.67k stars 525 forks source link

Fusing SLAM into Odom Topic output for EKF #481

Closed austin-InDro closed 2 years ago

austin-InDro commented 2 years ago

Feature description

Instead of the SLAM toolbox outputting to the TF node directly. Could there be an option to allow for a topic to publish to?

Implementation considerations

This would allow for intergration with the robot_localization package so that the calculated Odom position from the SLAM could be fused with other sensors.

SteveMacenski commented 2 years ago

You could add a parameter to disable it, sure

austin-InDro commented 2 years ago

Where in your code would I make that modification? I've been poking around slam_toolbox_common.cpp

Would I publish the read_pose variable?

Edit: Found the SynchronousSlamToolbox::run() looks like q_ queue of poses is a good bet for getting the current ish pose out. I'll attempt to make a modification there.

austin-InDro commented 2 years ago

Hi @SteveMacenski,

I haven't found a good place to implement this. Would you be able to help with some guidence on your code?

Thanks,

SteveMacenski commented 2 years ago

Add a parameter that would be put a boolean over this https://github.com/SteveMacenski/slam_toolbox/blob/ros2/src/slam_toolbox_common.cpp#L76-L78 to create the thread necessary to publish the transform.

Also, I think you can disable it now if you just set the transform_publish_period to 0, no?

austin-InDro commented 2 years ago

Ah I see, thanks. I did a much more hack method by placing some publishing code for a PoseWithCovariance in slam_toolbox_common.cpp. So I kept the thread, but commented out the publish transform message. I can go into more detail if you'd like, however I'm just currently away from my robot now. I can publish my modifications here next week.

I could attempt to implement it they way you mentioned above, then set up a PR request if you'd like?

SteveMacenski commented 2 years ago

Sure, sounds good! I'm always a fan of less hacky methods where possible :-)

austin-InDro commented 2 years ago

Hi Steve,

Sorry for the delayed response, I got pulled away to work on other projects. I will come back to this though! Up to you if you want to close the issue!

SteveMacenski commented 2 years ago

508 implements