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.

Sign up for GitHub

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

Jump to bottom

Added FromLLArray service #676

Open
wants to merge 3 commits into
base: foxy-devel
Choose a base branch
from
Open

Added FromLLArray service #676

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
89ca9bf
Added FromLLArray service
MarcinSlomiany Apr 22, 2021
cbcd769
User local coordinates also in toll services.
ursereg Dec 15, 2021
dc74221
FromLLArray - update after review
MarcinSlomiany May 8, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,7 @@ set(library_name rl_lib)

rosidl_generate_interfaces(${PROJECT_NAME}
"srv/FromLL.srv"
"srv/FromLLArray.srv"
"srv/GetState.srv"
"srv/SetDatum.srv"
"srv/SetPose.srv"
Expand Down
19 changes: 19 additions & 0 deletions include/robot_localization/navsat_transform.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,10 +36,13 @@
#include <robot_localization/srv/set_datum.hpp>
#include <robot_localization/srv/to_ll.hpp>
#include <robot_localization/srv/from_ll.hpp>
#include <robot_localization/srv/from_ll_array.hpp>

#include <Eigen/Dense>
#include <GeographicLib/Geocentric.hpp>
#include <GeographicLib/LocalCartesian.hpp>
#include <geographic_msgs/msg/geo_point.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
Expand All @@ -50,6 +53,7 @@
#include <tf2_ros/transform_listener.h>

#include <memory>
#include <stdexcept>
#include <string>

namespace robot_localization
Expand Down Expand Up @@ -99,6 +103,16 @@ class NavSatTransform : public rclcpp::Node
const std::shared_ptr<robot_localization::srv::FromLL::Request> request,
std::shared_ptr<robot_localization::srv::FromLL::Response> response);

//! @brief Callback for the from Lat Long Array service
//!
bool fromLLArrayCallback(
const std::shared_ptr<robot_localization::srv::FromLLArray::Request> request,
std::shared_ptr<robot_localization::srv::FromLLArray::Response> response);

//! @brief Method for convert point from Lat Lon to the map coordinates system
//!
geometry_msgs::msg::Point fromLL(const geographic_msgs::msg::GeoPoint & geo_point);

/**
* @brief Given the pose of the navsat sensor in the Cartesian frame, removes the
* offset from the vehicle's centroid and returns the Cartesian-frame pose of said
Expand Down Expand Up @@ -211,6 +225,11 @@ class NavSatTransform : public rclcpp::Node
*/
rclcpp::Service<robot_localization::srv::FromLL>::SharedPtr from_ll_srv_;

/**
* @brief Service for from Lat Long Array
*/
rclcpp::Service<robot_localization::srv::FromLLArray>::SharedPtr from_ll_array_srv_;

/**
* @brief Navsatfix publisher
*/
Expand Down
72 changes: 54 additions & 18 deletions src/navsat_transform.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -142,7 +142,8 @@ NavSatTransform::NavSatTransform(const rclcpp::NodeOptions & options)
"toLL", std::bind(&NavSatTransform::toLLCallback, this, _1, _2));
from_ll_srv_ = this->create_service<robot_localization::srv::FromLL>(
"fromLL", std::bind(&NavSatTransform::fromLLCallback, this, _1, _2));

from_ll_array_srv_ = this->create_service<robot_localization::srv::FromLLArray>(
"fromLLArray", std::bind(&NavSatTransform::fromLLArrayCallback, this, _1, _2));
std::vector<double> datum_vals;
if (use_manual_datum_) {
datum_vals = this->declare_parameter("datum", datum_vals);
Expand Down Expand Up @@ -407,9 +408,38 @@ bool NavSatTransform::fromLLCallback(
const std::shared_ptr<robot_localization::srv::FromLL::Request> request,
std::shared_ptr<robot_localization::srv::FromLL::Response> response)
{
double altitude = request->ll_point.altitude;
double longitude = request->ll_point.longitude;
double latitude = request->ll_point.latitude;
try {
response->map_point = fromLL(request->ll_point);
}
catch(const std::runtime_error& e) {
return false;
}

return true;
}

bool NavSatTransform::fromLLArrayCallback(
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) {
Copy link
Collaborator

@ayrton04 ayrton04 May 24, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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);

try {
response->map_points.push_back(fromLL(*it));
}
catch(const std::runtime_error& e) {
return false;
}
}

return true;
}

geometry_msgs::msg::Point NavSatTransform::fromLL(
const geographic_msgs::msg::GeoPoint& geo_point)
{
double altitude = geo_point.altitude;
double longitude = geo_point.longitude;
double latitude = geo_point.latitude;

tf2::Transform cartesian_pose;

Expand Down Expand Up @@ -437,15 +467,11 @@ bool NavSatTransform::fromLLCallback(

cartesian_pose.setOrigin(tf2::Vector3(cartesian_x, cartesian_y, altitude));

nav_msgs::msg::Odometry gps_odom;

if (!transform_good_) {
return false;
throw std::runtime_error("Invalid transform.");
}

response->map_point = cartesianToMap(cartesian_pose).pose.pose.position;

return true;
return cartesianToMap(cartesian_pose).pose.pose.position;
}

nav_msgs::msg::Odometry NavSatTransform::cartesianToMap(
Expand Down Expand Up @@ -485,14 +511,24 @@ void NavSatTransform::mapToLL(

odom_as_cartesian.mult(cartesian_world_trans_inverse_, pose);
odom_as_cartesian.setRotation(tf2::Quaternion::getIdentity());

// Now convert the data back to lat/long and place into the message
navsat_conversions::UTMtoLL(
odom_as_cartesian.getOrigin().getY(),
odom_as_cartesian.getOrigin().getX(),
utm_zone_,
latitude,
longitude);
if (use_local_cartesian_) {
gps_local_cartesian_.Reverse(
odom_as_cartesian.getOrigin().getX(),
odom_as_cartesian.getOrigin().getY(),
odom_as_cartesian.getOrigin().getZ(),
latitude,
longitude,
altitude
);
} else {
// Now convert the data back to lat/long and place into the message
navsat_conversions::UTMtoLL(
odom_as_cartesian.getOrigin().getY(),
odom_as_cartesian.getOrigin().getX(),
utm_zone_,
latitude,
longitude);
}
altitude = odom_as_cartesian.getOrigin().getZ();
}

Expand Down
3 changes: 3 additions & 0 deletions srv/FromLLArray.srv
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,3 @@
geographic_msgs/GeoPoint[] ll_points
---
geometry_msgs/Point[] map_points