tf2 for Melodic #755
tf2 for Melodic #755
Conversation
Cherry-pick a4a3da9 from ros-planning#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 ros-planning#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
This may have already been forward ported with PR ros-planning#728 But it was originally done as part of PR ros-planning#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
This finishes the compile errors from cherry-picking ros-planning#458. This commit ports the changes from ros-planning#601 to tf2.
|
:( |
|
Please pull the last commit off this -- it's passing CI without pthread it looks like (but not with it) |
moriarty
force-pushed
the
vrabuad-kinetic-tf2-on-melodic
branch
from
4161830
to
8b327b0
Compare
|
I've removed the last commit. On the robot there is a fresh 18.04 install and there the tests are passing. |
|
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? |
|
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. |
|
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. 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 👎 |
|
I tried #762 on top of this -- still doesn't seem to work |
|
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) |
|
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: |
|
When I put a goal in front of the robot, I get all 0 or 3.14 for rotation: |
|
@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). |
|
Once this is merged, I think we can remove the PCL depends from the cmake/package.xml -- I'll take care of that part. |
|
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. |
| const std::vector<geometry_msgs::PoseStamped>& global_plan, | ||
| const std::string& global_frame, | ||
| tf::Stamped<tf::Pose> &goal_pose); | ||
| geometry_msgs::PoseStamped &goal_pose); |
There was a problem hiding this comment.
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); |
There was a problem hiding this comment.
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) { |
|
I had a chance to test the latest version of this branch on the robot. Much better results 👍
|
Don't merge this until ros-planning/navigation#755 Updates fetch_depth_layer for tf2 changes coming upstream.
|
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 Awesome! |
[fetch_depth_layer][tf2] fixes for upstream navigation Changes for compatibility with ros-planning/navigation#755
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