ROS2 Navsat_transform_node in Dashing #476

klintan · 2019-08-05T01:54:36Z

I saw there was already an issue and a fork (#473) for Dashing, but made some changes remove all the deprecation warnings.

There might be some declare_parameters which could go back to get_parameter.

Also I guess it makes sense to create a new branch dashing-devel and if these changes makes sense, I'll change the PR to that branch instead.

ayrton04 · 2019-08-08T09:26:12Z

Please see my notes on that issue.

src/navsat_transform.cpp

src/ros_filter.cpp

klintan · 2019-08-09T05:04:32Z

It now compiles with navsat_transform_node. However there is one major issue, trying to run I get problems with the dynamically linked stuff:

dyld: Library not loaded: @loader_path/libnddsc.dylib
  Referenced from: /ros2/ros2-osx/lib/librosidl_typesupport_connext_c.dylib
  Reason: image not found

Which is really strange, because I couldn't see anything in particular in the CMakeLists file that would make a hard dependency on anything from connext (as it is an optional middleware).

I'll continue my research, but any input on that is welcome :). However I think its ready for some major PR feedback now.

Edit: Fixed!

ayrton04 · 2019-08-09T11:53:47Z

CMakeLists.txt

 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
-#find_package(geographic_msgs REQUIRED)
+find_package(geographic_msgs REQUIRED)


Happy to see this was released.

I don't think it's released yet though. I saw the comment in the issue. Right now it needs to be cloned manually and activate that workspace. This is kind of important as well ros-geographic-info/geographic_info#26 which hasn't been approved yet :(

uh, then we cant merge this @klintan, it'll not build in CI or on releasing debians

No worries, I'll try to push to get the geographic_msgs released, maybe we can keep this PR open until then (hopefully won't be months) and as soon as it's there, this will be mergeable.

edit: Sidenote, recently ROS2 converted ros-driver package for velodyne depends on the angles package https://github.com/ros/angles.git which is also not a part of the official release yet. But I get that different packages see this differently (of course regardless the geographic_info package at least needs the latest fixes in the ros2 branch)

sigh ok, now, weeks later, I have lobbied all the parties required to merge the ROS2 port PR, it will be released in the next dashing sync assuming I didnt mess it up. But now I have a new shiny GitHub organization on my profile, so not a total loss of time :-)
ros/rosdistro#22324

CMakeLists.txt

include/robot_localization/navsat_transform.hpp

src/navsat_transform.cpp

src/navsat_transform_node.cpp

rotu · 2019-08-09T16:25:42Z

It now compiles with navsat_transform_node. However there is one major issue, trying to run I get problems with the dynamically linked stuff:

I'm experiencing these issues as well with an unrelated package. I suspect this is caused by something upstream.

klintan · 2019-08-10T19:07:30Z

I'm experiencing these issues as well with an unrelated package. I suspect this is caused by something upstream.

Let me know if you find the/a solution, I'm going to try just removing the dynamic links using otool/install_name_tool . I'll let you know if I figure it out.

@rotu the issue was indeed upstream, it seems that geographic_msgs was exporting all of these dependencies

ament_export_dependencies(ament_cmake)
ament_export_dependencies(std_msgs)
ament_export_dependencies(geometry_msgs)
ament_export_dependencies(unique_identifier_msgs)
ament_export_dependencies(rosidl_default_generators)
ament_export_dependencies(rosidl_default_runtime)

Removing all of them except
ament_export_dependencies(rosidl_default_runtime)
fixes that.

IanTheEngineer · 2019-08-21T02:05:15Z

To test this PR, I made a vcstools file to include the unreleased upstream dependencies and open PR's:

navsat_transform_node.repos :

repositories:
  diagnostics:
    type: git
    url: git@github.com:ros/diagnostics.git
    version: ros2-devel
  geographic_info:
    type: git
    url: git@github.com:PickNikRobotics/geographic_info.git
    version: unique_identifier_msgs
  robot_localization:
    type: git
    url: git@github.com:klintan/robot_localization.git
    version: dashing-devel

After installing ROS Dashing, and python3-vcstools,

$ mkdir -p ~/ros2_ws/src
$ cd ~/ros2_ws
$ vcs import src < navsat_transform_node.repos
$ source /opt/ros/dashing/setup.bash
$ colcon build

IanTheEngineer · 2019-08-21T02:45:03Z

The package.xml isn't in this PR, but it needs to have geographic_msgs uncommented for this PR to begin to build with colcon :

-  <!--<depend>geographic_msgs</depend>-->
+  <depend>geographic_msgs</depend>

klintan · 2019-08-22T03:46:49Z

To test this PR, I made a vcstools file to include the unreleased upstream dependencies and open PR's:

navsat_transform_node.repos :

repositories:
  diagnostics:
    type: git
    url: git@github.com:ros/diagnostics.git
    version: ros2-devel
  geographic_info:
    type: git
    url: git@github.com:PickNikRobotics/geographic_info.git
    version: unique_identifier_msgs
  robot_localization:
    type: git
    url: git@github.com:klintan/robot_localization.git
    version: dashing-devel

After installing ROS Dashing, and python3-vcstools,

$ mkdir -p ~/ros2_ws/src
$ cd ~/ros2_ws
$ vcs import src < navsat_transform_node.repos
$ source /opt/ros/dashing/setup.bash
$ colcon build

That's awesome, thanks!

ayrton04 · 2019-09-23T09:39:30Z

Wow, thanks for all the effort here. I'll keep an eye out for the Dashing sync and merge afterwards, assuming nothing else needs to be done here.

SteveMacenski · 2019-09-23T14:27:49Z

I still see some issues, will do another pass through this afternoon.

SteveMacenski

Biggest issues around the queue_size & ros2 conventions

SteveMacenski · 2019-09-23T16:10:53Z

CMakeLists.txt

@@ -35,6 +39,7 @@ include_directories(
  include


I'm going to point you to one of my other packages for ros2 cmakelist best practices, this is really not easily maintainable with all the include / link dirs. If you set a variable with all the dependencies and you use ament, you can get rid of most of this code.

https://github.com/ros-planning/navigation2/blob/master/nav2_planner/CMakeLists.txt

SteveMacenski · 2019-09-23T16:13:17Z

include/robot_localization/navsat_transform.hpp

@@ -57,7 +57,7 @@ class NavSatTransform
  /**
   * @brief Constructor
   */
-  NavSatTransform();
+  NavSatTransform(std::shared_ptr<rclcpp::Node> node);


Best practices for this is that the NavSatTransform node should derive from rclcpp::Node, and when you make the shared_ptr of it in main that will deal with this, then all of your calls are working with methods available vs storing your node_. The ros nodehandle methodology is depreciated in ros2, its preferred to derive from the node to best make use of lifecycle and components, which is important for this package in particular

I actually tried to use that convention here as well, but I couldn't get it to work, always got the bad_weak_ptr exception during runtime because of the TF2 package (I think). The initializer lists wants a shared, and since there is now owner when it's passed I got that exception. However it could have been the way I used image_transport the "old" way, instead of the image_transport::create_subscription/create_publisher that made it fail, I can't fully recall I'll need to retest it.

If you make_shared the NavSatTransform object in main that will fix that. You get an issue with shared_ptrs if there's not already the reference counter set

src/navsat_transform.cpp

include/robot_localization/ros_filter.hpp

src/navsat_transform.cpp

src/ros_filter.cpp

src/navsat_transform.cpp

klintan · 2019-09-23T22:23:40Z

I thought some of the changes from the last PR was going to conflict with this one? Regardless I'll try to adopt the same style as that one for some of the changes here as well (and of course all of the review remarks)

Thanks for the very thorough review, I appreciate it and I appreciate your patience.

SteveMacenski · 2019-09-23T22:41:51Z

thorough

I think some of my colleagues would use another, less kind, word in its place, but thanks for baring with me 😸

klintan · 2019-10-02T05:08:02Z

Fixed a bunch of stuff, I will give the inherit from rclcpp::Node another try as well, hopefully I'll have time to get to that tomorrow night.

klintan · 2019-10-05T21:48:43Z

@SteveMacenski Now compiling and working using the ROS2 node-style.

Made most of the std::outs to RCLCPP style logging. On of the old std::outs outputted the pose though, which is kind of tricky to log using RCLCPP style. I don't think that one is necessary though, but maybe long term for debugging purposes/brevity we want to get it in there.

Let me know if you find anything, any and all feedback is welcome :)

SteveMacenski

Some small fish changes needed

src/navsat_transform_node.cpp

src/navsat_transform.cpp

SteveMacenski · 2019-10-08T00:12:19Z

src/navsat_transform.cpp

-  rclcpp::Rate rate(frequency);
-  while (rclcpp::ok()) {
-    rclcpp::spin_some(node_);
+  int interval = (1/frequency) * 1000;  


you have frequency defaulted to 10, so this would be... 100 hz? I think you mean to move s to ms but the variable name didnt make that clear, can you change the name or do the ms conversion in the chrono call? Or better yet, use chrono::seconds(interval)

Yes, good catch. I'll keep the variable name, but I like the approach of chrono::seconds(interval)
to make it more clear.

klintan · 2019-10-23T04:27:09Z

@SteveMacenski fixed the things you noted (I hope :) ). However I noticed another issue when fixing that, line 954 in ros_filter.cpp (same thing in 2 other places in ros_filter), perhaps you can take an extra look at that so it looks sane. I'm open to any other approach.

Sorry for the delay.

reso1 · 2019-10-23T11:57:53Z

Hello @klintan, I've been using your "beta version" of the navsat_transform node, and I found a problem when using a dual_ekf_navsat configurations, a local ekf node and a global ekf node with GPS (following the yaml below: dual_ekf_navsat_example.yaml

SO here is the problem, the navsat_transform node subscribes to "/odometry/filtered" topic msgs, and because both "ekf_se_odom"(local) node and "ekf_se_map"(global) node will publish "/odometry/filtered" topic msg, it will cause the "/odometry/gps" topic msg published by navsat_transform node jump between 2 values, each calculated based on the msg published by "ekf_se_odom"(local) node or and "ekf_se_map"(global) node, respectively.

from this workflow figure, the navsat_transfrom_node only takes the "/odometry/filtered" msg published by *”kf_localization_node(map)“ into account.

But anyway I've made a workaround, by only executing "odomCallback" function when the frame_id of the "/odometry/filtered" msg header eqauls to "map"(i.e. published by "ekf_se_map"(global) node).

I'm wondering that whether I'm not correctly using the "dual_ekf_navsat" configurations with GPS info, or is there a better solution for this problem? Thanks!

ayrton04 · 2019-10-23T12:09:14Z

@reso1, the two EKFs should not be publishing on the same topic. Remap the tier 1 (odom) frame one to something else.

SteveMacenski · 2019-10-23T16:16:19Z

@klintan I’ll take a look this afternoon.

I (still, frustratingly, annoyingly, {other more colorful words}) do not have a robot to meaningfully test this on, so by in large I’m trusting beyond the code semantics that this actually works.

I’m not sure if Tom has a ROS2-able robot to have a quick third party check, or @reso1 other than your topic remapping (see Tom’s comment), is this working for you? Have you tried all the params getting to you & outputs as expected?

SteveMacenski

Themes:

still need queue sizes
A few more updates to CMakeList still necessary
Member variable naming
Just making sure defaults are same as in ROS1

SteveMacenski · 2019-10-23T20:25:14Z

CMakeLists.txt

@@ -19,37 +23,60 @@ find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(Eigen3 REQUIRED)

+set(includes


This isn't necessary anymore, use ament_target_dependencies see examples here https://github.com/ros-planning/navigation2/blob/master/nav2_planner/CMakeLists.txt

Hm I think I might have missed something, because couldn't get it to find some includes (eigen3 and diagnostics_updated without adding them).

See my changes, I think I need some help on that one.

Sure let me know

Of everything I'm the least concerned by the CMake stuff, give it another shot and if not I can take care of it for you after the fact

CMakeLists.txt

include/robot_localization/navsat_transform.hpp

src/navsat_transform.cpp

src/ros_filter.cpp

SteveMacenski · 2019-10-23T20:42:32Z

src/ros_filter.cpp

@@ -1436,7 +1445,7 @@ void RosFilter::loadParams()
      deceleration_gains);

    control_sub_ = node_->create_subscription<geometry_msgs::msg::Twist>(
-      "cmd_vel",
+      "cmd_vel", 10, 


This parameter would be outside the scope of odom, twist, pose and imu. Do we want to declare a separate parameter for this one?

Oh sorry this one is fine. I went a little trigger happy when I saw the number 10 😉 Though I think 1 is also the ROS1 default, if you see how its used, there's no benefit to storing anything except the most recent one

haha :) ok I'll change this to 1 as well.

src/ros_filter.cpp

reso1 · 2019-10-24T01:40:20Z

@ayrton04 @SteveMacenski
Thanks for your advices, I've tried the remapping solution and now it works fine :)

klintan · 2019-10-24T03:06:37Z

@ayrton04 @SteveMacenski
Thanks for your advices, I've tried the remapping solution and now it works fine :)

Happy to hear it's working for you :)

SteveMacenski · 2019-10-24T03:07:57Z

Wow that was quick, ok I think the last thing is just the conversation on the parameters for checking _N+1

I'm not buying this set(libraries ...) call as necessary,but if that's the worst thing in here, I can live with that.

SteveMacenski

With that, I give this my (fairly meaningless) thumbs up.

Lets see what Tom says on the undeclare thing. Otherwise, assuming that you've sufficiently tested this since it doesn't appear the automated testing pipeline is in good shape, I would be OK merging this.

Thanks with sticking with my rounds-and-rounds of nitpicks

blacksoul000 · 2019-10-25T07:05:16Z

src/ros_filter.cpp

@@ -1849,11 +1881,11 @@ void RosFilter::run()

    double diag_duration = (cur_time - last_diag_time).nanoseconds();
    if (print_diagnostics_ &&
-    		(diag_duration >= diagnostic_updater_.getPeriod() ||
-    				diag_duration < 0.0))
+        (diag_duration >= diagnostic_updater_.getPeriod() ||


diagnostic_updater_.getPeriod().nanoseconds()
?

getPeriod() is already returning nanoseconds :)

edit: as seen below, I didn't have the latest diagnostics_updater, thanks for pointing this out!

I am looking at the diagnostic repo, and getPeriod() returns a rcl_duration_value_t in the head of the crystal branch , but returns a rclcpp::Duration at the head of the dashing branch. rclcpp::Duration does need a .nanoseconds() to get the types matching

it looks like the type change came in with c272eb276d of the diagnostic repo dashing branch and is in the released debian for dashing

aah ok, yes I did not pull the absolute latest, I'll do that and re-run. I think the one I had was at least dashing, so maybe that commit was aimed at one of the patch-releases.

klintan · 2019-11-19T07:54:56Z

Resolving merge conflicts. I glanced through about half of them, they're really easy, just alot of them

Thanks for your patience, took a while to knock it out. Fixed a bunch of linting issues that failed in the tests. Let me know if you see anything I missed after/during the rebase.

SteveMacenski · 2019-11-19T15:53:23Z

@Jconn are your concerns resolved?

Jconn · 2019-11-19T16:39:59Z

@Jconn are your concerns resolved?

No more concerns, good work @klintan

SteveMacenski · 2019-11-19T16:41:37Z

I dont believe it. Its finally in!

ayrton04 · 2019-11-19T16:42:02Z

Epic. Thanks to everyone involved!

SteveMacenski reviewed Aug 8, 2019

View reviewed changes

src/navsat_transform.cpp Outdated Show resolved Hide resolved

src/ros_filter.cpp Outdated Show resolved Hide resolved

ayrton04 reviewed Aug 9, 2019

View reviewed changes

SteveMacenski mentioned this pull request Aug 9, 2019

Changes needed for Dashing #478

Closed

klintan changed the title ~~ROS2 Dashing updates~~ ROS2 Navsat_transform_node in Dashing Aug 10, 2019

SteveMacenski requested changes Sep 23, 2019

View reviewed changes

SteveMacenski approved these changes Oct 8, 2019

View reviewed changes

SteveMacenski requested changes Oct 23, 2019

View reviewed changes

SteveMacenski approved these changes Oct 24, 2019

View reviewed changes

ayrton04 mentioned this pull request Oct 24, 2019

Getting navsat_transform node to build #498

Closed

blacksoul000 reviewed Oct 25, 2019

View reviewed changes

klintan and others added 23 commits November 12, 2019 22:49

navsat_transform_node compiles

cc4abdb

PR feedback changes

4249bed

reformatting and added parameter queue_size

a227832

fixed some tests

3364b50

removed forgotten debug message

29d60e6

removed another forgotten debug message

e686657

update for diagnostics updater

bba9a4c

PR feedback and some refactoring

2bf909e

removed one std printout

12f2ee4

ROS2 inheritance style instead of node handle

8a25155

fixed PR feedback and some other small issues fixed

01c15ec

PR feedback changes

7970c9c

minor changes from PR feedback

7948031

undeclare unused parameters

0c77ee8

new diagnostics_updater changes, and fixed bug caught in review

2b543da

SensorDataQoS and custom queue_size for navsat_transform node

08591bb

removed queue_size parameter in navsat_transform

6c6cb3e

fixed major bug causing infinite loops

5accf78

updated QoS

612a955

changed to sensordataqos

f3b2607

some param getter fixes

cf466e5

linting fixes mostly for tests to pass

58c9f7c

some minor rebasing mistakes and small fixes

de7a293

Jconn approved these changes Nov 19, 2019

View reviewed changes

SteveMacenski merged commit d471205 into cra-ros-pkg:dashing-devel Nov 19, 2019

ROS2 Navsat_transform_node in Dashing #476

ROS2 Navsat_transform_node in Dashing #476

Conversation

klintan commented Aug 5, 2019

ayrton04 commented Aug 8, 2019

klintan commented Aug 9, 2019 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

klintan Aug 10, 2019 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

rotu commented Aug 9, 2019

klintan commented Aug 10, 2019 • edited Loading

IanTheEngineer commented Aug 21, 2019

IanTheEngineer commented Aug 21, 2019 • edited Loading

klintan commented Aug 22, 2019

ayrton04 commented Sep 23, 2019

SteveMacenski commented Sep 23, 2019

SteveMacenski 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

klintan commented Sep 23, 2019

SteveMacenski commented Sep 23, 2019

klintan commented Oct 2, 2019

klintan commented Oct 5, 2019

SteveMacenski left a comment

Choose a reason for hiding this comment

SteveMacenski Oct 8, 2019 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

klintan commented Oct 23, 2019

reso1 commented Oct 23, 2019

ayrton04 commented Oct 23, 2019

SteveMacenski commented Oct 23, 2019

SteveMacenski 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

SteveMacenski Oct 24, 2019 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

reso1 commented Oct 24, 2019

klintan commented Oct 24, 2019

SteveMacenski commented Oct 24, 2019

SteveMacenski left a comment • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

klintan Oct 26, 2019 • edited Loading

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

klintan commented Nov 19, 2019

SteveMacenski commented Nov 19, 2019

Jconn commented Nov 19, 2019

SteveMacenski commented Nov 19, 2019

ayrton04 commented Nov 19, 2019

klintan commented Aug 9, 2019 •

edited

Loading

klintan Aug 10, 2019 •

edited

Loading

klintan commented Aug 10, 2019 •

edited

Loading

IanTheEngineer commented Aug 21, 2019 •

edited

Loading

SteveMacenski Oct 8, 2019 •

edited

Loading

SteveMacenski Oct 24, 2019 •

edited

Loading

SteveMacenski left a comment •

edited

Loading

klintan Oct 26, 2019 •

edited

Loading