plan_route_action_client::PlanRouteActionClient Class Reference

Action client node for planning a route. More...

#include <plan_route_action_client.hpp>

Inheritance diagram for plan_route_action_client::PlanRouteActionClient:

Public Member Functions
	PlanRouteActionClient ()
	Constructor.

Private Types
using	PlanRoute = route_planning_msgs::action::PlanRoute
using	GoalHandlePlanRoute = rclcpp_action::ClientGoalHandle<PlanRoute>

Private Member Functions
template<typename T >
void	declareAndLoadParameter (const std::string &name, T &param, const std::string &description, const bool add_to_auto_reconfigurable_params=true, const bool is_required=false, const bool read_only=false, const std::optional< double > &from_value=std::nullopt, const std::optional< double > &to_value=std::nullopt, const std::optional< double > &step_value=std::nullopt, const std::string &additional_constraints="")
	Declares and loads a ROS parameter.
rcl_interfaces::msg::SetParametersResult	parametersCallback (const std::vector< rclcpp::Parameter > &parameters)
	Handles reconfiguration when a parameter value is changed.
void	setup ()
	Sets up subscribers, publishers, etc. to configure the node.
void	goalPoseCallback (const geometry_msgs::msg::PoseStamped::SharedPtr msg)
	Callback for goal pose (most likely received from RViz)
void	autoPlanningTimerCallback ()
	Callback for automatically planning a route, e.g., if waypoints are given.
void	planToNextWaypoint ()
	Plans to next waypoint.
void	planToRandomDestination ()
	Plans to a random destination.
void	sendGoal (const geometry_msgs::msg::PoseStamped::SharedPtr msg, const std::vector< geometry_msgs::msg::PointStamped > &intermediate_destinations={})
	Sends a goal to the action server.
void	goalResponseCallback (const GoalHandlePlanRoute::SharedPtr &goal_handle)
	Callback for goal response from the action server.
void	feedbackCallback (GoalHandlePlanRoute::SharedPtr goal_handle, const std::shared_ptr< const PlanRoute::Feedback > feedback)
	Callback for feedback from the action server.
void	resultCallback (const GoalHandlePlanRoute::WrappedResult &result)
	Callback for result from the action server.

Private Attributes
std::vector< std::tuple< std::string, std::function< void(const rclcpp::Parameter &)> > >	auto_reconfigurable_params_
	Auto-reconfigurable parameters for dynamic reconfiguration.
OnSetParametersCallbackHandle::SharedPtr	parameters_callback_
	Callback handle for dynamic parameter reconfiguration.
rclcpp::Subscription< geometry_msgs::msg::PoseStamped >::SharedPtr	goal_pose_subscriber_
	Subscriber for goal pose.
rclcpp_action::Client< PlanRoute >::SharedPtr	action_client_
	Action client.
std::shared_future< GoalHandlePlanRoute::SharedPtr >	goal_handle_future_
	Goal handle.
rclcpp::TimerBase::SharedPtr	auto_planning_timer_
	Timer to automatically plan route, e.g., if waypoints are given.
std::unique_ptr< Lanelet2MapInterface >	ll2_interface_
	Lanelet2 map interface.
std::vector< std::pair< double, double > >	waypoints_
	WGS84 waypoints to endlessly follow.
std::vector< double >	waypoint_wait_times_
	Wait time for each waypoint [s]; negative values mark intermediate destinations.
size_t	next_waypoint_idx_ = 0
	Index of next waypoint to follow.
bool	has_active_waypoint_ = false
	Whether the active goal belongs to the waypoint list.
double	active_waypoint_wait_time_s_ = 0.0
	Wait time of the active waypoint [s].
double	auto_planning_resume_time_s_ = 0.0
	Earliest wall-clock time at which automatic planning may continue.
bool	has_completed_one_goal_ = false
	Whether one goal has been completed (succeeded or failed)
std::string	ll2_map_server_name_ = "ll2_map_server"
	Name of lanelet2_map_server node (parameter)
std::vector< std::string >	waypoints_param_
	WGS84 waypoints to endlessly follow (parameter)
bool	enable_random_destination_ = false
	Whether to plan a route to a random destination (parameter)
bool	enable_continuous_planning_ = false
	Whether to continuously plan a new route (parameter)
bool	cancel_route_ = false
	Flag to cancel the route planning action (parameter)

Detailed Description

Action client node for planning a route.

Definition at line 33 of file plan_route_action_client.hpp.

Member Typedef Documentation

GoalHandlePlanRoute

using plan_route_action_client::PlanRouteActionClient::GoalHandlePlanRoute = rclcpp_action::ClientGoalHandle<PlanRoute>

private

Definition at line 35 of file plan_route_action_client.hpp.

PlanRoute

using plan_route_action_client::PlanRouteActionClient::PlanRoute = route_planning_msgs::action::PlanRoute

private

Definition at line 34 of file plan_route_action_client.hpp.

Constructor & Destructor Documentation

PlanRouteActionClient()

plan_route_action_client::PlanRouteActionClient::PlanRouteActionClient

(

)

Constructor.

Definition at line 67 of file plan_route_action_client.cpp.

                                             : Node("plan_route_action_client") {
  this->declareAndLoadParameter("ll2_map_server_name", ll2_map_server_name_, "Name of lanelet2_map_server node", false);
  this->declareAndLoadParameter(
      "waypoints", waypoints_param_,
      "List of WGS84 waypoints to follow (list of strings with comma-separated '<LATITUDE>,<LONGITUDE>[,<WAIT_TIME_S>]', missing "
      "wait time defaults to 0s, negative wait time means intermediate destination)",
      true);
  this->declareAndLoadParameter("enable_random_destination", enable_random_destination_,
                                "Whether to plan a route to a random destination", true);
  this->declareAndLoadParameter("enable_continuous_planning", enable_continuous_planning_,
                                "Whether to continuously plan a new route (either looping waypoints or to a random destination)",
                                true);
  this->declareAndLoadParameter("cancel_route", cancel_route_, "Cancel active route planning action (to be set at runtime)",
                                true);
  this->setup();
}

Member Function Documentation

autoPlanningTimerCallback()

void plan_route_action_client::PlanRouteActionClient::autoPlanningTimerCallback ( )

private

Callback for automatically planning a route, e.g., if waypoints are given.

Does nothing, if no waypoints are given and random planning is disabled. Precedence: random planning, waypoints

Definition at line 243 of file plan_route_action_client.cpp.

                                                      {
  const double now_s = std::chrono::duration<double>(std::chrono::steady_clock::now().time_since_epoch()).count();
  if (now_s < auto_planning_resume_time_s_) {
    return;
  }
 
  if (enable_random_destination_ && (enable_continuous_planning_ || !has_completed_one_goal_)) {
    this->planToRandomDestination();
  } else if (!waypoints_.empty()) {
    if (next_waypoint_idx_ >= waypoints_.size() && enable_continuous_planning_) {
      next_waypoint_idx_ = 0;  // loop waypoints, if continuous planning is enabled
    }
    if (next_waypoint_idx_ < waypoints_.size()) {
      this->planToNextWaypoint();
    }
  } else {
    RCLCPP_DEBUG(this->get_logger(), "Nothing to plan, waiting for waypoints or random destination");
  }
}

declareAndLoadParameter()

template<typename T >

void plan_route_action_client::PlanRouteActionClient::declareAndLoadParameter ( const std::string & name, T & param, const std::string & description, const bool add_to_auto_reconfigurable_params = true, const bool is_required = false, const bool read_only = false, const std::optional< double > & from_value = std::nullopt, const std::optional< double > & to_value = std::nullopt, const std::optional< double > & step_value = std::nullopt, const std::string & additional_constraints = "" )

private

Declares and loads a ROS parameter.

Parameters

[in]	name	name
[in]	param	parameter variable to load into
[in]	description	description
[in]	add_to_auto_reconfigurable_params	enable reconfiguration of parameter
[in]	is_required	whether failure to load parameter will stop node
[in]	read_only	set parameter to read-only
[in]	from_value	parameter range minimum
[in]	to_value	parameter range maximum
[in]	step_value	parameter range step
[in]	additional_constraints	additional constraints description

Definition at line 85 of file plan_route_action_client.cpp.

                                                                                             {
  rcl_interfaces::msg::ParameterDescriptor param_desc;
  param_desc.description = description;
  param_desc.additional_constraints = additional_constraints;
  param_desc.read_only = read_only;
 
  auto type = rclcpp::ParameterValue(param).get_type();
 
  if (from_value.has_value() && to_value.has_value()) {
    if constexpr (std::is_integral_v<T>) {
      rcl_interfaces::msg::IntegerRange range;
      T step = static_cast<T>(step_value.has_value() ? step_value.value() : 1);
      range.set__from_value(static_cast<T>(from_value.value())).set__to_value(static_cast<T>(to_value.value())).set__step(step);
      param_desc.integer_range = {range};
    } else if constexpr (std::is_floating_point_v<T>) {
      rcl_interfaces::msg::FloatingPointRange range;
      T step = static_cast<T>(step_value.has_value() ? step_value.value() : 1.0);
      range.set__from_value(static_cast<T>(from_value.value())).set__to_value(static_cast<T>(to_value.value())).set__step(step);
      param_desc.floating_point_range = {range};
    } else {
      RCLCPP_WARN(this->get_logger(), "Parameter type of parameter '%s' does not support specifying a range", name.c_str());
    }
  }
 
  this->declare_parameter(name, type, param_desc);
 
  try {
    param = this->get_parameter(name).get_value<T>();
    std::stringstream ss;
    ss << "Loaded parameter '" << name << "': ";
    if constexpr (is_vector_v<T>) {
      ss << "[";
      for (const auto& element : param) ss << element << (&element != &param.back() ? ", " : "]");
    } else {
      ss << param;
    }
    RCLCPP_INFO_STREAM(this->get_logger(), ss.str());
  } catch (rclcpp::exceptions::ParameterUninitializedException&) {
    if (is_required) {
      RCLCPP_FATAL_STREAM(this->get_logger(), "Missing required parameter '" << name << "', exiting");
      exit(EXIT_FAILURE);
    } else {
      std::stringstream ss;
      ss << "Missing parameter '" << name << "', using default value: ";
      if constexpr (is_vector_v<T>) {
        ss << "[";
        for (const auto& element : param) ss << element << (&element != &param.back() ? ", " : "]");
      } else {
        ss << param;
      }
      RCLCPP_WARN_STREAM(this->get_logger(), ss.str());
      this->set_parameters({rclcpp::Parameter(name, rclcpp::ParameterValue(param))});
    }
  }
 
  if (add_to_auto_reconfigurable_params) {
    std::function<void(const rclcpp::Parameter&)> setter = [&param](const rclcpp::Parameter& p) { param = p.get_value<T>(); };
    auto_reconfigurable_params_.push_back(std::make_tuple(name, setter));
  }
}

feedbackCallback()

void plan_route_action_client::PlanRouteActionClient::feedbackCallback ( GoalHandlePlanRoute::SharedPtr goal_handle, const std::shared_ptr< const PlanRoute::Feedback > feedback )

private

Callback for feedback from the action server.

Parameters

[in]	goal_handle	goal handle
[in]	feedback	action feedback

Definition at line 429 of file plan_route_action_client.cpp.

                                                                                                    {
  (void)goal_handle;
 
  const double distance_traveled = feedback->distance_traveled;
  const double distance_total = feedback->distance_remaining + feedback->distance_traveled;
  rclcpp::Duration time_traveled(feedback->time_traveled.sec, feedback->time_traveled.nanosec);
  rclcpp::Duration time_remaining(feedback->time_remaining.sec, feedback->time_remaining.nanosec);
  rclcpp::Duration time_total = time_traveled + time_remaining;
  RCLCPP_INFO(this->get_logger(), "Route progress: %.2f / %.2f m, %.1f / %.1f s", distance_traveled, distance_total,
              time_traveled.seconds(), time_total.seconds());
}

goalPoseCallback()

void plan_route_action_client::PlanRouteActionClient::goalPoseCallback ( const geometry_msgs::msg::PoseStamped::SharedPtr msg )

private

Callback for goal pose (most likely received from RViz)

Parameters

[in]

msg

goal pose

Definition at line 234 of file plan_route_action_client.cpp.

                                                                                           {
  RCLCPP_INFO(this->get_logger(), "Received goal pose (%.3f, %.3f, %.3f) in frame '%s'", msg->pose.position.x,
              msg->pose.position.y, msg->pose.position.z, msg->header.frame_id.c_str());
  has_active_waypoint_ = false;
  active_waypoint_wait_time_s_ = 0.0;
  auto_planning_resume_time_s_ = 0.0;
  sendGoal(msg);
}

goalResponseCallback()

void plan_route_action_client::PlanRouteActionClient::goalResponseCallback ( const GoalHandlePlanRoute::SharedPtr & goal_handle )

private

Callback for goal response from the action server.

Parameters

[in]

goal_handle

goal handle

Definition at line 419 of file plan_route_action_client.cpp.

                                                                                                {
  if (!goal_handle) {
    RCLCPP_ERROR(this->get_logger(), "Goal rejected by action server");
    has_active_waypoint_ = false;
    auto_planning_timer_->reset();  // restart auto-planning timer
  } else {
    RCLCPP_INFO(this->get_logger(), "Goal accepted by action server");
  }
}

parametersCallback()

rcl_interfaces::msg::SetParametersResult plan_route_action_client::PlanRouteActionClient::parametersCallback ( const std::vector< rclcpp::Parameter > & parameters )

private

Handles reconfiguration when a parameter value is changed.

Parameters

[in]

parameters

parameters

Returns

parameter change result

Definition at line 155 of file plan_route_action_client.cpp.

                                                  {
  for (const auto& param : parameters) {
    for (auto& auto_reconfigurable_param : auto_reconfigurable_params_) {
      if (param.get_name() == std::get<0>(auto_reconfigurable_param)) {
        std::get<1>(auto_reconfigurable_param)(param);
        RCLCPP_INFO(this->get_logger(), "Reconfigured parameter '%s'", param.get_name().c_str());
        break;
      }
    }
 
    // handle waypoints
    if (param.get_name() == "waypoints") {
      auto parsed_waypoints = parseWaypoints(waypoints_param_, waypoint_wait_times_, this->get_logger());
      if (parsed_waypoints) {
        waypoints_ = *parsed_waypoints;
      } else {
        std::stringstream ss;
        ss << "Failed to parse parameter 'waypoints': [";
        for (const auto& waypoint : waypoints_param_) {
          ss << waypoint << (&waypoint != &waypoints_param_.back() ? ", " : "]");
        }
        RCLCPP_ERROR(this->get_logger(), "%s", ss.str().c_str());
      }
    }
 
    // handle cancel_route
    if (param.get_name() == "cancel_route") {
      if (cancel_route_) {
        if (action_client_->wait_for_action_server(std::chrono::duration<double>(0.1))) {
          RCLCPP_INFO(this->get_logger(), "Cancelling route");
          action_client_->async_cancel_all_goals();
        } else {
          RCLCPP_WARN(this->get_logger(), "Action server not available, cannot cancel route");
        }
      }
    }
  }
 
  rcl_interfaces::msg::SetParametersResult result;
  result.successful = true;
 
  return result;
}

planToNextWaypoint()

void plan_route_action_client::PlanRouteActionClient::planToNextWaypoint ( )

private

Plans to next waypoint.

Definition at line 263 of file plan_route_action_client.cpp.

                                               {
  // check if waypoint is valid
  if (next_waypoint_idx_ >= waypoints_.size()) {
    RCLCPP_ERROR(this->get_logger(), "Waypoint index %ld out of bounds (%ld), skipping", next_waypoint_idx_, waypoints_.size());
    return;
  }
  if (waypoint_wait_times_.size() != waypoints_.size()) {
    RCLCPP_ERROR(this->get_logger(), "Waypoint wait times do not match waypoints, skipping");
    return;
  }
 
  // check if map is loaded
  if (!ll2_interface_->map_loaded_) {
    RCLCPP_ERROR(this->get_logger(), "Map not loaded, cannot generate waypoint");
    return;
  }
 
  // generate goal pose from waypoint
  auto goal_pose = std::make_shared<geometry_msgs::msg::PoseStamped>();
  std::vector<geometry_msgs::msg::PointStamped> intermediate_destinations;
  auto ll2_projector = ll2_interface_->getProjectorPtr();
  if (!ll2_projector) {
    RCLCPP_ERROR(this->get_logger(), "Failed to generate waypoint goal pose");
    return;
  }
 
  size_t checked_waypoints = 0;
  while (checked_waypoints < waypoints_.size()) {
    const auto& waypoint = waypoints_[next_waypoint_idx_];
    const double wait_time_s = waypoint_wait_times_[next_waypoint_idx_];
    lanelet::GPSPoint gps_waypoint;
    gps_waypoint.lat = waypoint.first;
    gps_waypoint.lon = waypoint.second;
    lanelet::BasicPoint3d map_waypoint = ll2_projector->forward(gps_waypoint);
 
    geometry_msgs::msg::PointStamped waypoint_point;
    waypoint_point.header.frame_id = ll2_interface_->map_frame_id_;
    waypoint_point.header.stamp = this->now();
    waypoint_point.point.x = map_waypoint.x();
    waypoint_point.point.y = map_waypoint.y();
    waypoint_point.point.z = 0.0;
 
    if (wait_time_s < 0.0) {
      RCLCPP_INFO(this->get_logger(), "Adding intermediate waypoint (%.6f, %.6f)", waypoint.first, waypoint.second);
      intermediate_destinations.push_back(waypoint_point);
      next_waypoint_idx_++;
      if (next_waypoint_idx_ >= waypoints_.size()) {
        if (enable_continuous_planning_) {
          next_waypoint_idx_ = 0;
        } else {
          RCLCPP_ERROR(this->get_logger(), "No destination waypoint found after intermediate waypoints");
          return;
        }
      }
      checked_waypoints++;
      continue;
    }
 
    RCLCPP_INFO(this->get_logger(), "Planning route to next waypoint (%.6f, %.6f)", waypoint.first, waypoint.second);
    goal_pose->pose.position = waypoint_point.point;
    goal_pose->header = waypoint_point.header;
    active_waypoint_wait_time_s_ = wait_time_s;
    next_waypoint_idx_++;
    break;
  }
 
  // send goal
  if (!goal_pose->header.frame_id.empty()) {
    RCLCPP_INFO(this->get_logger(), "Generated waypoint goal pose (%.3f, %.3f, %.3f) in frame '%s'", goal_pose->pose.position.x,
                goal_pose->pose.position.y, goal_pose->pose.position.z, goal_pose->header.frame_id.c_str());
    auto_planning_timer_->cancel();  // cancel auto-planning timer until goal completion
    has_active_waypoint_ = true;
    this->sendGoal(goal_pose, intermediate_destinations);
  } else {
    RCLCPP_ERROR(this->get_logger(), "Failed to generate waypoint goal pose");
  }
}

planToRandomDestination()

void plan_route_action_client::PlanRouteActionClient::planToRandomDestination ( )

private

Plans to a random destination.

Definition at line 341 of file plan_route_action_client.cpp.

                                                    {
  RCLCPP_INFO(this->get_logger(), "Planning route to random destination");
 
  // check if map is loaded
  if (!ll2_interface_->map_loaded_) {
    RCLCPP_ERROR(this->get_logger(), "Map not loaded, cannot generate a random destination");
    return;
  }
 
  // generate random goal pose by sampling a random lanelet
  auto goal_pose = std::make_shared<geometry_msgs::msg::PoseStamped>();
  lanelet::LaneletMapConstPtr map = ll2_interface_->getMapPtr();
  if (!map->laneletLayer.empty()) {
    const auto lanelet_count = static_cast<int>(map->laneletLayer.size());
    const auto random_lanelet_idx =
        static_cast<std::iterator_traits<decltype(map->laneletLayer.begin())>::difference_type>(std::rand() % lanelet_count);
    auto random_lanelet = *std::next(map->laneletLayer.begin(), random_lanelet_idx);
    auto centerline = random_lanelet.centerline();
    if (!centerline.empty()) {
      auto point = centerline.back();
      goal_pose->pose.position.x = point.x();
      goal_pose->pose.position.y = point.y();
      goal_pose->pose.position.z = point.z();
      if (centerline.size() > 1) {
        auto heading =
            std::atan2(point.y() - centerline[centerline.size() - 2].y(), point.x() - centerline[centerline.size() - 2].x());
        tf2::Quaternion q;
        q.setRPY(0, 0, heading);
        goal_pose->pose.orientation = tf2::toMsg(q);
      }
      goal_pose->header.frame_id = ll2_interface_->map_frame_id_;
      goal_pose->header.stamp = this->now();
    }
  }
 
  // send goal
  if (!goal_pose->header.frame_id.empty()) {
    RCLCPP_INFO(this->get_logger(), "Generated random goal pose (%.3f, %.3f, %.3f) in frame '%s'", goal_pose->pose.position.x,
                goal_pose->pose.position.y, goal_pose->pose.position.z, goal_pose->header.frame_id.c_str());
    auto_planning_timer_->cancel();  // cancel auto-planning timer until goal completion
    has_active_waypoint_ = false;
    active_waypoint_wait_time_s_ = 0.0;
    this->sendGoal(goal_pose);
  } else {
    RCLCPP_ERROR(this->get_logger(), "Failed to generate random goal pose");
  }
}

resultCallback()

void plan_route_action_client::PlanRouteActionClient::resultCallback ( const GoalHandlePlanRoute::WrappedResult & result )

private

Callback for result from the action server.

Parameters

[in]

result

action result

Definition at line 442 of file plan_route_action_client.cpp.

                                                                                         {
  const double distance_traveled = result.result->distance_traveled;
  const builtin_interfaces::msg::Duration& time_traveled = result.result->time_traveled;
  const double wait_time_s = has_active_waypoint_ ? active_waypoint_wait_time_s_ : 0.0;
 
  if (result.code == rclcpp_action::ResultCode::SUCCEEDED) {
    if (result.result->destination_reached) {
      RCLCPP_INFO(this->get_logger(), "Goal succeeded: destination reached after %.2fm and %ds", distance_traveled,
                  time_traveled.sec);
    } else {
      RCLCPP_WARN(this->get_logger(), "Goal succeeded, but destination not reached after %.2fm and %ds", distance_traveled,
                  time_traveled.sec);
    }
  } else if (result.code == rclcpp_action::ResultCode::CANCELED) {
    RCLCPP_WARN(this->get_logger(), "Goal canceled: traveled %.2fm and %ds", distance_traveled, time_traveled.sec);
  } else if (result.code == rclcpp_action::ResultCode::ABORTED) {
    RCLCPP_ERROR(this->get_logger(), "Goal aborted: traveled %.2fm and %ds", distance_traveled, time_traveled.sec);
  } else {
    RCLCPP_ERROR(this->get_logger(), "Goal finished with unknown result code: %d", static_cast<int>(result.code));
  }
 
  has_active_waypoint_ = false;
  has_completed_one_goal_ = true;
  if (result.code == rclcpp_action::ResultCode::SUCCEEDED && wait_time_s > 0.0) {
    RCLCPP_INFO(this->get_logger(), "Waiting %.2fs before planning next waypoint", wait_time_s);
    const double now_s = std::chrono::duration<double>(std::chrono::steady_clock::now().time_since_epoch()).count();
    auto_planning_resume_time_s_ = now_s + wait_time_s;
  }
  auto_planning_timer_->reset();  // restart auto-planning timer
}

sendGoal()

void plan_route_action_client::PlanRouteActionClient::sendGoal ( const geometry_msgs::msg::PoseStamped::SharedPtr msg, const std::vector< geometry_msgs::msg::PointStamped > & intermediate_destinations = {} )

private

Sends a goal to the action server.

Parameters

[in]	msg	goal pose
[in]	intermediate_destinations	intermediate destinations to route via

Definition at line 389 of file plan_route_action_client.cpp.

                                                                                                               {
  RCLCPP_INFO(this->get_logger(), "Requesting to plan route to destination (%.3f, %.3f, %.3f) in frame '%s'",
              msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, msg->header.frame_id.c_str());
 
  // check if action server is available
  if (!action_client_->wait_for_action_server(std::chrono::duration<double>(0.1))) {
    RCLCPP_ERROR(this->get_logger(), "Action server not available, aborting");
    has_active_waypoint_ = false;
    auto_planning_timer_->reset();  // restart auto-planning timer
    return;
  }
 
  // build goal
  auto goal = PlanRoute::Goal();
  goal.destination = geometry_msgs::msg::PointStamped();
  goal.destination.header = msg->header;
  goal.destination.point = msg->pose.position;
  goal.intermediate_destinations = intermediate_destinations;
 
  // send goal
  auto send_goal_options = rclcpp_action::Client<PlanRoute>::SendGoalOptions();
  send_goal_options.goal_response_callback = std::bind(&PlanRouteActionClient::goalResponseCallback, this, std::placeholders::_1);
  send_goal_options.feedback_callback =
      std::bind(&PlanRouteActionClient::feedbackCallback, this, std::placeholders::_1, std::placeholders::_2);
  send_goal_options.result_callback = std::bind(&PlanRouteActionClient::resultCallback, this, std::placeholders::_1);
  goal_handle_future_ = action_client_->async_send_goal(goal, send_goal_options);
  RCLCPP_INFO(this->get_logger(), "Goal sent");
}

setup()

void plan_route_action_client::PlanRouteActionClient::setup ( )

private

Sets up subscribers, publishers, etc. to configure the node.

Definition at line 200 of file plan_route_action_client.cpp.

                                  {
  // callback for dynamic parameter configuration
  parameters_callback_ =
      this->add_on_set_parameters_callback(std::bind(&PlanRouteActionClient::parametersCallback, this, std::placeholders::_1));
 
  // subscriber for goal pose
  goal_pose_subscriber_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
      "~/goal_pose", 10, std::bind(&PlanRouteActionClient::goalPoseCallback, this, std::placeholders::_1));
  RCLCPP_INFO(this->get_logger(), "Subscribed to '%s'", goal_pose_subscriber_->get_topic_name());
 
  // action client
  action_client_ = rclcpp_action::create_client<PlanRoute>(this, "/planning/lanelet2_route_planning/plan_route");
 
  // ll2 map interface
  ll2_interface_ = std::make_unique<Lanelet2MapInterface>(*this, ll2_map_server_name_);
 
  // parse waypoints
  auto parsed_waypoints = parseWaypoints(waypoints_param_, waypoint_wait_times_, this->get_logger());
  if (parsed_waypoints) {
    waypoints_ = *parsed_waypoints;
  } else {
    std::stringstream ss;
    ss << "Failed to parse parameter 'waypoints': [";
    for (const auto& waypoint : waypoints_param_) {
      ss << waypoint << (&waypoint != &waypoints_param_.back() ? ", " : "]");
    }
    RCLCPP_ERROR(this->get_logger(), "%s", ss.str().c_str());
  }
 
  // set up auto-planning timer
  auto_planning_timer_ = this->create_wall_timer(std::chrono::milliseconds(1000),
                                                 std::bind(&PlanRouteActionClient::autoPlanningTimerCallback, this));
}

Member Data Documentation

action_client_

rclcpp_action::Client<PlanRoute>::SharedPtr plan_route_action_client::PlanRouteActionClient::action_client_

private

Action client.

Definition at line 157 of file plan_route_action_client.hpp.

active_waypoint_wait_time_s_

double plan_route_action_client::PlanRouteActionClient::active_waypoint_wait_time_s_ = 0.0

private

Wait time of the active waypoint [s].

Definition at line 197 of file plan_route_action_client.hpp.

auto_planning_resume_time_s_

double plan_route_action_client::PlanRouteActionClient::auto_planning_resume_time_s_ = 0.0

private

Earliest wall-clock time at which automatic planning may continue.

Definition at line 202 of file plan_route_action_client.hpp.

auto_planning_timer_

rclcpp::TimerBase::SharedPtr plan_route_action_client::PlanRouteActionClient::auto_planning_timer_

private

Timer to automatically plan route, e.g., if waypoints are given.

Definition at line 167 of file plan_route_action_client.hpp.

auto_reconfigurable_params_

std::vector<std::tuple<std::string, std::function<void(const rclcpp::Parameter&)> > > plan_route_action_client::PlanRouteActionClient::auto_reconfigurable_params_

private

Auto-reconfigurable parameters for dynamic reconfiguration.

Definition at line 142 of file plan_route_action_client.hpp.

cancel_route_

bool plan_route_action_client::PlanRouteActionClient::cancel_route_ = false

private

Flag to cancel the route planning action (parameter)

Definition at line 236 of file plan_route_action_client.hpp.

enable_continuous_planning_

bool plan_route_action_client::PlanRouteActionClient::enable_continuous_planning_ = false

private

Whether to continuously plan a new route (parameter)

Either to the next waypoint or to a random destination, if enabled

Definition at line 231 of file plan_route_action_client.hpp.

enable_random_destination_

bool plan_route_action_client::PlanRouteActionClient::enable_random_destination_ = false

private

Whether to plan a route to a random destination (parameter)

Definition at line 224 of file plan_route_action_client.hpp.

goal_handle_future_

std::shared_future<GoalHandlePlanRoute::SharedPtr> plan_route_action_client::PlanRouteActionClient::goal_handle_future_

private

Goal handle.

Definition at line 162 of file plan_route_action_client.hpp.

goal_pose_subscriber_

rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr plan_route_action_client::PlanRouteActionClient::goal_pose_subscriber_

private

Subscriber for goal pose.

Definition at line 152 of file plan_route_action_client.hpp.

has_active_waypoint_

bool plan_route_action_client::PlanRouteActionClient::has_active_waypoint_ = false

private

Whether the active goal belongs to the waypoint list.

Definition at line 192 of file plan_route_action_client.hpp.

has_completed_one_goal_

bool plan_route_action_client::PlanRouteActionClient::has_completed_one_goal_ = false

private

Whether one goal has been completed (succeeded or failed)

Definition at line 207 of file plan_route_action_client.hpp.

ll2_interface_

std::unique_ptr<Lanelet2MapInterface> plan_route_action_client::PlanRouteActionClient::ll2_interface_

private

Lanelet2 map interface.

Definition at line 172 of file plan_route_action_client.hpp.

ll2_map_server_name_

std::string plan_route_action_client::PlanRouteActionClient::ll2_map_server_name_ = "ll2_map_server"

private

Name of lanelet2_map_server node (parameter)

Definition at line 212 of file plan_route_action_client.hpp.

next_waypoint_idx_

size_t plan_route_action_client::PlanRouteActionClient::next_waypoint_idx_ = 0

private

Index of next waypoint to follow.

Definition at line 187 of file plan_route_action_client.hpp.

parameters_callback_

OnSetParametersCallbackHandle::SharedPtr plan_route_action_client::PlanRouteActionClient::parameters_callback_

private

Callback handle for dynamic parameter reconfiguration.

Definition at line 147 of file plan_route_action_client.hpp.

waypoint_wait_times_

std::vector<double> plan_route_action_client::PlanRouteActionClient::waypoint_wait_times_

private

Wait time for each waypoint [s]; negative values mark intermediate destinations.

Definition at line 182 of file plan_route_action_client.hpp.

waypoints_

std::vector<std::pair<double, double> > plan_route_action_client::PlanRouteActionClient::waypoints_

private

WGS84 waypoints to endlessly follow.

Definition at line 177 of file plan_route_action_client.hpp.

waypoints_param_

std::vector<std::string> plan_route_action_client::PlanRouteActionClient::waypoints_param_

private

WGS84 waypoints to endlessly follow (parameter)

list of strings with comma-separated '<LATITUDE>,<LONGITUDE>[,<WAIT_TIME_S>]'

Definition at line 219 of file plan_route_action_client.hpp.

---

The documentation for this class was generated from the following files:

• plan_route_action_client/include/plan_route_action_client/plan_route_action_client.hpp
• plan_route_action_client/src/plan_route_action_client.cpp