Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

tf2 for Melodic #755

Conversation

Projects
None yet
3 participants
@moriarty
Copy link
Contributor

commented Jul 15, 2018

This cherry-picks the two commits from @vrabaud for #458.

Then I fixed the compile errors and rebased onto melodic after #752 was merged today.
There were lots of conflicts because some things were already ported to tf2.

The recent changes for #601
0bd8f30
caused compile errors because the code wasn't there at the time of #458. But the code is nearly duplicate of the getRobotPose function in CostMaps

I will test this more on a Robot today/tomorrow

vrabaud and others added some commits Jan 26, 2015

[tf2] [WIP] cherry-pick: convert to tf2
Cherry-pick a4a3da9 from #458 & @vrabaud

Original PR branch was aimed at Kinect, thus a straight cherry-pick
does not compile.
This fixes all conflicts detected by Git.

Not Compiling: move_base, global_planner.
Due to changes merged since the original #458.
I will fix those issues in a new commit.

 Conflicts:
	amcl/package.xml
	amcl/src/amcl_node.cpp
	amcl/test/basic_localization.py
	base_local_planner/include/base_local_planner/local_planner_util.h
	base_local_planner/package.xml
	base_local_planner/src/local_planner_util.cpp
	base_local_planner/src/trajectory_planner_ros.cpp
	carrot_planner/package.xml
	clear_costmap_recovery/package.xml
	costmap_2d/CMakeLists.txt
	costmap_2d/include/costmap_2d/costmap_2d_ros.h
	costmap_2d/package.xml
	costmap_2d/src/costmap_2d_ros.cpp
	costmap_2d/test/footprint_tests.cpp
	dwa_local_planner/package.xml
	fake_localization/fake_localization.cpp
	fake_localization/package.xml
	global_planner/package.xml
	move_base/package.xml
	move_base/src/move_base.cpp
	nav_core/package.xml
	navfn/package.xml
	robot_pose_ekf/CMakeLists.txt
	robot_pose_ekf/include/robot_pose_ekf/odom_estimation.h
	robot_pose_ekf/include/robot_pose_ekf/odom_estimation_node.h
	robot_pose_ekf/package.xml
	robot_pose_ekf/src/odom_estimation.cpp
	robot_pose_ekf/src/odom_estimation_node.cpp
	rotate_recovery/package.xml
	rotate_recovery/src/rotate_recovery.cpp
[tf2] cherry-pick: amcl stripSlash
This may have already been forward ported with PR #728
But it was originally done as part of PR #458.

Which is why there was a conflict.
I am cherry-picking for completeness.

get code to work with frames starting with "/"
This is to get test_rosie_multilaser.xml to work

 Conflicts:
	amcl/src/amcl_node.cpp
[tf2][Move Base] finish compile error fixes
This finishes the compile errors from cherry-picking #458.
This commit ports the changes from #601 to tf2.
@moriarty

This comment has been minimized.

Copy link
Contributor Author

commented Jul 15, 2018

:(

01:53:55 [ 47%] Built target navfn_generate_messages_py
01:53:55 [ 58%] Built target navfn_generate_messages_nodejs
01:53:55 [ 70%] Built target navfn_generate_messages_lisp
01:53:56 [ 88%] Built target navfn_generate_messages_eus
01:53:56 [ 88%] Built target navfn_generate_messages
01:53:56 Scanning dependencies of target navfn_node
01:53:56 [ 94%] Building CXX object CMakeFiles/navfn_node.dir/src/navfn_node.cpp.o
01:54:00 Build timed out (after 120 minutes). Marking the build as failed.
01:54:00 Build was aborted
01:54:00 [WARNINGS] Skipping publisher since build result is FAILURE
01:54:00 [xUnit] [INFO] - Starting to record.
@mikeferguson

This comment has been minimized.

Copy link
Member

commented Jul 15, 2018

Please pull the last commit off this -- it's passing CI without pthread it looks like (but not with it)

@moriarty moriarty force-pushed the moriarty:vrabuad-kinetic-tf2-on-melodic branch from 4161830 to 8b327b0 Jul 15, 2018

@moriarty

This comment has been minimized.

Copy link
Contributor Author

commented Jul 15, 2018

I've removed the last commit. On the robot there is a fresh 18.04 install and there the tests are passing.

@moriarty

This comment has been minimized.

Copy link
Contributor Author

commented Jul 16, 2018

The tests did pass earlier for the same commit but now they time out again.

I'm testing this on the robot and not liking it, but it could be a problem with my local changes inside of fetch_depth_layer...

It is trying to rotate and clear the costmap, when the costmap is clear, and then not driving.

Could you add a label Work In Progress. or Do Not Merge?

@moriarty

This comment has been minimized.

Copy link
Contributor Author

commented Jul 16, 2018

Thanks for re-starting the test. I opened #758 after looking into it for a bit. But I don't know why they are slower.

@moriarty

This comment has been minimized.

Copy link
Contributor Author

commented Jul 16, 2018

screenshot from 2018-07-15 22-02-23
screenshot from 2018-07-15 22-02-53
screenshot from 2018-07-15 22-07-31

Something is wonky with the local costmap, it's clear, but the robot thinks it isn't, and then does recovery behaviour. If you send it a very straight goal it seems to do better but will eventually need to rotate towards the end and then likely get stuck.

[ WARN] [1531717342.372912205]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1531717352.452888662]: Rotate recovery behavior started.
[ WARN] [1531717368.096294805]: Clearing costmap to unstuck robot (0.500000m).
[ERROR] [1531717378.176300087]: Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors

I won't close this PR right away but unless we can get some more testing I don't think this will make it into Melodic 👎

@mikeferguson

This comment has been minimized.

Copy link
Member

commented Jul 17, 2018

I tried #762 on top of this -- still doesn't seem to work

@mikeferguson

This comment has been minimized.

Copy link
Member

commented Jul 17, 2018

Oh -- and the failed behavior is very similar to what I remember from about 1-2 years ago when Vincent first opened the PR. So it's not something new, it's probably the same issue (I just still can't tell what it is -- appears to be in the controllers, since the global plan generation looks the same as before, but we can't get a trajectory which leads us into recovery behaviors)

@mikeferguson

This comment has been minimized.

Copy link
Member

commented Jul 17, 2018

Changing the log levels shows that base_local_planner is generating 0,0,0 as the only valid velocity (all the time) when I put a goal BEHIND the robot:

[DEBUG] [1531844563.980617596, 3278.463000000]: A valid velocity command of (0.00, 0.00, 0.00) was found for this cycle.
[ERROR] [1531844563.995207335, 3278.478000000]: Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors
@mikeferguson

This comment has been minimized.

Copy link
Member

commented Jul 17, 2018

When I put a goal in front of the robot, I get all 0 or 3.14 for rotation:

[DEBUG] [1531844750.472033751, 3460.743000000]: A valid velocity command of (0.75, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844750.511759053, 3460.782000000]: A valid velocity command of (0.75, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844750.548593117, 3460.819000000]: A valid velocity command of (0.78, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844750.587950656, 3460.857000000]: A valid velocity command of (0.86, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844750.628661779, 3460.897000000]: A valid velocity command of (0.89, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844750.676566303, 3460.944000000]: A valid velocity command of (0.92, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844750.708943731, 3460.975000000]: A valid velocity command of (0.92, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844750.752152409, 3461.018000000]: A valid velocity command of (1.00, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844750.795496217, 3461.059000000]: A valid velocity command of (1.04, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844750.832951118, 3461.097000000]: A valid velocity command of (1.22, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844750.880555572, 3461.143000000]: A valid velocity command of (1.08, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844750.921056461, 3461.183000000]: A valid velocity command of (0.83, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844750.962401396, 3461.224000000]: A valid velocity command of (0.93, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844750.997112083, 3461.257000000]: A valid velocity command of (1.00, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844751.038590689, 3461.299000000]: A valid velocity command of (0.91, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844751.081272262, 3461.340000000]: A valid velocity command of (1.00, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844751.124439335, 3461.382000000]: A valid velocity command of (1.10, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844751.158775676, 3461.416000000]: A valid velocity command of (1.20, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844751.201225997, 3461.456000000]: A valid velocity command of (1.32, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844751.249137630, 3461.500000000]: A valid velocity command of (1.44, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844751.285482205, 3461.539000000]: A valid velocity command of (1.43, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844751.325692704, 3461.578000000]: A valid velocity command of (1.40, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844751.373146100, 3461.625000000]: A valid velocity command of (0.00, 0.00, 3.14) was found for this cycle.
[DEBUG] [1531844751.407364463, 3461.658000000]: A valid velocity command of (1.34, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844751.447512926, 3461.699000000]: A valid velocity command of (1.31, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844751.490793554, 3461.741000000]: A valid velocity command of (1.28, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844751.529950491, 3461.779000000]: A valid velocity command of (1.26, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844751.568020781, 3461.817000000]: A valid velocity command of (1.21, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844751.620724050, 3461.866000000]: A valid velocity command of (1.18, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844751.652220121, 3461.897000000]: A valid velocity command of (1.14, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844751.696154974, 3461.940000000]: A valid velocity command of (1.11, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844751.733137588, 3461.974000000]: A valid velocity command of (1.06, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844751.775924147, 3462.019000000]: A valid velocity command of (1.02, 0.00, 0.00) was found for this cycle.
[DEBUG] [1531844751.827375754, 3462.070000000]: A valid velocity command of (1.00, 0.00, 0.00) was found for this cycle.
@mikeferguson

This comment has been minimized.

Copy link
Member

commented Jul 18, 2018

@moriarty There are four instances of setEuler(z, x, y) in this PR -- changing them to setRPY(x, y, z) makes at least the base_local_planner work (haven't tested DWA).

@mikeferguson

This comment has been minimized.

Copy link
Member

commented Jul 18, 2018

Once this is merged, I think we can remove the PCL depends from the cmake/package.xml -- I'll take care of that part.

@mikeferguson mikeferguson self-assigned this Jul 18, 2018

@moriarty

This comment has been minimized.

Copy link
Contributor Author

commented Jul 18, 2018

Sorry, I got sick so I couldn't submit a PR sooner.

I haven't tested this last commit on the robot but I made the setEuler->setRPY change.

@moriarty
Copy link
Contributor Author

left a comment

I just reviewed the diff again to check for anything odd. mainly whitespace, but one const removal jumped out.

const std::vector<geometry_msgs::PoseStamped>& global_plan,
const std::string& global_frame,
tf::Stamped<tf::Pose> &goal_pose);
geometry_msgs::PoseStamped &goal_pose);

This comment has been minimized.

Copy link
@moriarty

moriarty Jul 18, 2018

Author Contributor

todo: fix whitespace


bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);

bool getLocalPlan(const tf::Stamped<tf::Pose>& global_pose, std::vector<geometry_msgs::PoseStamped>& transformed_plan);
bool getLocalPlan(geometry_msgs::PoseStamped& global_pose, std::vector<geometry_msgs::PoseStamped>& transformed_plan);

This comment has been minimized.

Copy link
@moriarty

moriarty Jul 18, 2018

Author Contributor

todo: no longer const?

Trajectory TrajectoryPlanner::findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel,
tf::Stamped<tf::Pose>& drive_velocities){
Trajectory TrajectoryPlanner::findBestPath(const geometry_msgs::PoseStamped& global_pose,
geometry_msgs::PoseStamped& global_vel, geometry_msgs::PoseStamped& drive_velocities) {

This comment has been minimized.

Copy link
@moriarty

moriarty Jul 18, 2018

Author Contributor

todo: whitespace

@moriarty

This comment has been minimized.

Copy link
Contributor Author

commented Jul 18, 2018

I had a chance to test the latest version of this branch on the robot. Much better results 👍

screenshot from 2018-07-18 10-41-25
screenshot from 2018-07-18 10-41-46
screenshot from 2018-07-18 10-42-00
screenshot from 2018-07-18 10-42-31
screenshot from 2018-07-18 10-42-43
screenshot from 2018-07-18 10-43-07
screenshot from 2018-07-18 10-43-18
screenshot from 2018-07-18 10-43-33
screenshot from 2018-07-18 10-43-54

@moriarty moriarty referenced this pull request Jul 18, 2018

Closed

Support for Ubuntu 18.04 #63

moriarty added a commit to moriarty/fetch_ros that referenced this pull request Jul 18, 2018

[fetch_depth_layer][tf2] ros-planning/navigation#755
Don't merge this until ros-planning/navigation#755

Updates fetch_depth_layer for tf2 changes coming upstream.
@moriarty

This comment has been minimized.

Copy link
Contributor Author

commented Jul 19, 2018

@reinzor & @Rayman, I didn't rebase this branch to contain your PR, but are you able to test this one too and ensure it doesn't break anything?

@mikeferguson

This comment has been minimized.

Copy link
Member

commented Jul 21, 2018

Ok -- I know this hasn't gotten total testing on all platforms -- but I think we at least can be certain the API changes are correct for TF2. Any other bugs that creep up can thus be fixed in future releases.

There's a lot of folks waiting on us getting navigation into debians, so I'm going to merge this now, and move towards a release.

@mikeferguson mikeferguson merged commit 5fe0878 into ros-planning:melodic-devel Jul 21, 2018

1 check passed

Mpr__navigation__ubuntu_bionic_amd64 Build finished.
Details
@moriarty

This comment has been minimized.

Copy link
Contributor Author

commented Jul 21, 2018

🍾 🎉 👍 @mikeferguson Awesome!

moriarty added a commit to fetchrobotics/fetch_ros that referenced this pull request Aug 23, 2018

Merge pull request #81 from moriarty/tf2-nav-melodic-devel
[fetch_depth_layer][tf2] fixes for upstream navigation

Changes for compatibility with ros-planning/navigation#755

@JWhitleyAStuff JWhitleyAStuff referenced this pull request Feb 26, 2019

Merged

Fix/melodic #200

timonegk pushed a commit to bit-bots/dwa_local_planner that referenced this pull request May 3, 2019

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.