plan_route_action_client Namespace Reference

Classes
struct	is_vector
struct	is_vector< std::vector< T, A > >
class	PlanRouteActionClient
	Action client node for planning a route. More...

Functions
std::optional< std::vector< std::pair< double, double > > >	parseWaypoints (const std::vector< std::string > &waypoints_param, std::vector< double > &waypoint_wait_times, const rclcpp::Logger &logger)
	Parses WGS84 waypoints from "<LATITUDE>,<LONGITUDE>[,<WAIT_TIME_S>]" strings.

Variables
template<typename C >
constexpr bool	is_vector_v = is_vector<C>::value

Function Documentation

parseWaypoints()

std::optional< std::vector< std::pair< double, double > > > plan_route_action_client::parseWaypoints	(	const std::vector< std::string > &	waypoints_param,
		std::vector< double > &	waypoint_wait_times,
		const rclcpp::Logger &	logger )

Parses WGS84 waypoints from "<LATITUDE>,<LONGITUDE>[,<WAIT_TIME_S>]" strings.

Parameters

[in]	waypoints_param	waypoint parameter values
[in,out]	waypoint_wait_times	wait time for each waypoint
[in]	logger	ROS logger

Returns

parsed latitude/longitude pairs, or std::nullopt if parsing fails

Definition at line 27 of file plan_route_action_client.cpp.

                                                                                                 {
  std::vector<std::pair<double, double>> waypoints;
  std::vector<double> parsed_wait_times;
  for (const auto& waypoint : waypoints_param) {
    size_t comma_pos = waypoint.find(',');
    if (comma_pos != std::string::npos) {
      try {
        size_t wait_time_comma_pos = waypoint.find(',', comma_pos + 1);
        if (wait_time_comma_pos != std::string::npos && waypoint.find(',', wait_time_comma_pos + 1) != std::string::npos) {
          return std::nullopt;
        }
        double lat = std::stod(waypoint.substr(0, comma_pos));
        double lon = std::stod(waypoint.substr(comma_pos + 1, wait_time_comma_pos - comma_pos - 1));
        double wait_time_s = 0.0;
        if (wait_time_comma_pos != std::string::npos) {
          wait_time_s = std::stod(waypoint.substr(wait_time_comma_pos + 1));
        }
        waypoints.emplace_back(lat, lon);
        parsed_wait_times.push_back(wait_time_s);
      } catch (const std::invalid_argument& e) {
        return std::nullopt;
      } catch (const std::out_of_range& e) {
        return std::nullopt;
      }
    } else {
      return std::nullopt;
    }
  }
 
  if (!parsed_wait_times.empty() && parsed_wait_times.back() < 0.0) {
    RCLCPP_WARN(logger, "Last waypoint cannot be intermediate, treating it as stop with wait_time_s 0.0");
    parsed_wait_times.back() = 0.0;
  }
 
  waypoint_wait_times = parsed_wait_times;
  return waypoints;
}

Variable Documentation

is_vector_v

template<typename C >

bool plan_route_action_client::is_vector_v = is_vector<C>::value

inlineconstexpr

Definition at line 28 of file plan_route_action_client.hpp.