Added FromLLArray service #676
Conversation
src/navsat_transform.cpp
Outdated
| cartesian_z); | ||
| } else { | ||
|
|
||
| navsat_conversions::LLtoUTM( |
There was a problem hiding this comment.
I thought I deprecated these functions in favor of geographilib. But I cannot confirm until next week.
There was a problem hiding this comment.
Yes, we should be consistent with whatever method we're using.
There was a problem hiding this comment.
Found it!
Looks like parts didn't make it into ROS2. The reasoning of using geographiclib directly instead of these navsat_transform functions is here: https://github.com/cra-ros-pkg/robot_localization/pull/627/files#r598612280
srv/FromLLArray.srv
Outdated
| @@ -0,0 +1,3 @@ | |||
| geographic_msgs/GeoPoint[] ll_points | |||
| --- | |||
| geometry_msgs/Point[] map_points | |||
src/navsat_transform.cpp
Outdated
| cartesian_z); | ||
| } else { | ||
|
|
||
| navsat_conversions::LLtoUTM( |
There was a problem hiding this comment.
Yes, we should be consistent with whatever method we're using.
src/navsat_transform.cpp
Outdated
|
|
||
| cartesian_pose.setOrigin(tf2::Vector3(cartesian_x, cartesian_y, altitude)); | ||
|
|
||
| // nav_msgs::msg::Odometry gps_odom; // it's unused? |
There was a problem hiding this comment.
Please remove this line.
Yes it's repeated |
|
Apologies for the ping, but is there any chance we'll address the PR feedback on this? |
|
Any update here? No worries if there's no interest in addressing the feedback, but I'll close the PR in that case. |
|
Hi! I was not aware of this PR, I had opened #724 requesting exactly the same feature. I may pick this one up and address your comments soon, however I could not give you an specific time frame |
|
Hi, sorry for the long delay. I think next week I will start making changes based on your comments @ayrton04. Hope to finish it soon :) |
|
Just checking in on this again. As before, no rush. |
|
Ping! :) |
|
Hi, I think that before this week ends I'll start pushing some commits.. |
|
Finally I've made update to this PR: piappl@dc74221 If this approach is fine I'll do the same for the toLL method. Also I'll get rid of an additional commit of my colleague. |
| const std::shared_ptr<robot_localization::srv::FromLLArray::Request> request, | ||
| std::shared_ptr<robot_localization::srv::FromLLArray::Response> response) | ||
| { | ||
| for (auto it = request->ll_points.begin(); it != request->ll_points.end(); ++it) { |
There was a problem hiding this comment.
So the behavior here is not ideal, in that if one of the points throws an exception, the response object might be partially filled out. What we'd really want there is an "all or nothing" operation in which we return all points if they all succeed, otherwise we return an empty vector.
In this case, we know that fromLL will only throw an exception if !transform_good_, so if any one point throws an exception, they all will. However, we don't want to assume we know how fromLL works internally here (and someone might change it later on). I might suggest something like this (note that I have not tested this):
#include <algorithm>
...
decltype(response->map_points) converted_points;
converted_points.reserve(request->ll_points.size());
try
{
std::transform(
request->ll_points.begin(),
request->ll_points.end(),
std::back_inserter(response->map_points),
[] (const auto& point) { return fromLL(point); });
}
catch(...)
{
return false;
}
response->map_points = std::move(converted_points);
|
Is this expected to be merged at some point? I almost came to implement this all over :) |
|
See my comment above. If the change I suggested is implemented, then I will happily merge. |
|
@MarcinSlomiany are you up to it? Want to give me write permissions to your fork or should I copy your changes as a base to my fork? |
|
Hi @YossiO-BWR! I can't give you write permissions becaus it's my company repo but |
|
@MarcinSlomiany Understandable, lookg forward seeing this merged! |
Service allows to quickly transform multiple ll points to ros coordinates system.