#include <waypoints_navigator.hpp>

Inheritance diagram for cl_nav2z::WaypointNavigator:

Collaboration diagram for cl_nav2z::WaypointNavigator:

Public Member Functions
	WaypointNavigator ()

void	onInitialize () override

template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()

void	loadWayPointsFromFile (std::string filepath)

void	loadWayPointsFromFile2 (std::string filepath)

void	setWaypoints (const std::vector< geometry_msgs::msg::Pose > &waypoints)

void	setWaypoints (const std::vector< Pose2D > &waypoints)

std::optional< std::shared_future< std::shared_ptr< rclcpp_action::ClientGoalHandle< nav2_msgs::action::NavigateToPose > > > >	sendNextGoal (std::optional< NavigateNextWaypointOptions > options=std::nullopt, cl_nav2z::ClNav2Z::SmaccNavigateResultSignal::WeakPtr callback=cl_nav2z::ClNav2Z::SmaccNavigateResultSignal::WeakPtr())

void	stopWaitingResult ()

const std::vector< geometry_msgs::msg::Pose > &	getWaypoints () const

const std::vector< std::string > &	getWaypointNames () const

std::optional< geometry_msgs::msg::Pose >	getNamedPose (std::string name) const

long	getCurrentWaypointIndex () const

std::optional< std::string >	getCurrentWaypointName () const

void	rewind (int count)

void	forward (int count)

void	seekName (std::string name)

Public Member Functions inherited from smacc2::ISmaccComponent
	ISmaccComponent ()

virtual	~ISmaccComponent ()

virtual std::string	getName () const

Public Attributes
WaypointEventDispatcher	waypointsEventDispatcher

ClNav2Z *	client_

long	currentWaypoint_

smacc2::SmaccSignal< void()>	onNavigationRequestSucceded

smacc2::SmaccSignal< void()>	onNavigationRequestAborted

smacc2::SmaccSignal< void()>	onNavigationRequestCancelled

Private Member Functions
void	insertWaypoint (int index, geometry_msgs::msg::Pose &newpose)

void	removeWaypoint (int index)

void	onNavigationResult (const ClNav2Z::WrappedResult &r)

void	onGoalReached (const ClNav2Z::WrappedResult &res)

void	onGoalCancelled (const ClNav2Z::WrappedResult &)

void	onGoalAborted (const ClNav2Z::WrappedResult &)

Private Attributes
std::vector< geometry_msgs::msg::Pose >	waypoints_

std::vector< std::string >	waypointsNames_

boost::signals2::connection	succeddedNav2ZClientConnection_

boost::signals2::connection	abortedNav2ZClientConnection_

boost::signals2::connection	cancelledNav2ZClientConnection_

Additional Inherited Members
Protected Member Functions inherited from smacc2::ISmaccComponent
virtual void	onInitialize ()

template<typename EventType >
void	postEvent (const EventType &ev)

template<typename EventType >
void	postEvent ()

template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()

template<typename TComponent >
void	requiresComponent (TComponent *&requiredComponentStorage, bool throwExceptionIfNotExist=false)

template<typename TComponent >
void	requiresComponent (std::string name, TComponent *&requiredComponentStorage, bool throwExceptionIfNotExist=false)

template<typename TClient >
void	requiresClient (TClient *&requiredClientStorage)

template<typename SmaccComponentType , typename TOrthogonal , typename TClient , typename... TArgs>
SmaccComponentType *	createSiblingComponent (TArgs... targs)

template<typename SmaccComponentType , typename TOrthogonal , typename TClient , typename... TArgs>
SmaccComponentType *	createSiblingNamedComponent (std::string name, TArgs... targs)

rclcpp::Node::SharedPtr	getNode ()

rclcpp::Logger	getLogger () const

ISmaccStateMachine *	getStateMachine ()

Protected Attributes inherited from smacc2::ISmaccComponent
ISmaccStateMachine *	stateMachine_

ISmaccClient *	owner_

Detailed Description

Definition at line 55 of file waypoints_navigator.hpp.

Constructor & Destructor Documentation

◆ WaypointNavigator()

cl_nav2z::WaypointNavigator::WaypointNavigator ( )

Definition at line 37 of file waypoints_navigator.cpp.

37: currentWaypoint_(0), waypoints_(0) {}

cl_nav2z::WaypointNavigator::waypoints_

std::vector< geometry_msgs::msg::Pose > waypoints_

Definition: waypoints_navigator.hpp:118

cl_nav2z::WaypointNavigator::currentWaypoint_

long currentWaypoint_

Definition: waypoints_navigator.hpp:96

Member Function Documentation

◆ forward()

void cl_nav2z::WaypointNavigator::forward ( int count )

Definition at line 73 of file waypoints_navigator.cpp.

{
  currentWaypoint_++;
  if (currentWaypoint_ >= (long)waypoints_.size() - 1)
    currentWaypoint_ = (long)waypoints_.size() - 1;
}

References currentWaypoint_, and waypoints_.

Referenced by cl_nav2z::CbSeekWaypoint::onEntry().

Here is the caller graph for this function:

◆ getCurrentWaypointIndex()

long cl_nav2z::WaypointNavigator::getCurrentWaypointIndex ( ) const

Definition at line 356 of file waypoints_navigator.cpp.

356{ return currentWaypoint_; }

References currentWaypoint_.

Referenced by cl_nav2z::CbNavigateNextWaypoint::onEntry(), cl_nav2z::CbNavigateNextWaypointUntilReached::onNavigationActionSuccess(), and cl_nav2z::CpWaypointsVisualizer::update().

Here is the caller graph for this function:

◆ getCurrentWaypointName()

std::optional< std::string > cl_nav2z::WaypointNavigator::getCurrentWaypointName ( ) const

Definition at line 347 of file waypoints_navigator.cpp.

{
  if (currentWaypoint_ >= 0 && currentWaypoint_ < (int)waypointsNames_.size())
  {
    return waypointsNames_[currentWaypoint_];
  }
  return std::nullopt;
}

References currentWaypoint_, and waypointsNames_.

Referenced by cl_nav2z::CbNavigateNextWaypoint::onEntry(), and cl_nav2z::CbNavigateNextWaypointUntilReached::onNavigationActionSuccess().

Here is the caller graph for this function:

◆ getNamedPose()

std::optional< geometry_msgs::msg::Pose > cl_nav2z::WaypointNavigator::getNamedPose ( std::string name ) const

Definition at line 326 of file waypoints_navigator.cpp.

{
  if (this->waypointsNames_.size() > 0)
  {
    for (int i = 0; i < (int)this->waypointsNames_.size(); i++)
    {
      if (this->waypointsNames_[i] == name)
      {
        return this->waypoints_[i];
      }
    }
  }
 
  return std::nullopt;
}

References waypoints_, and waypointsNames_.

◆ getWaypointNames()

const std::vector< std::string > & cl_nav2z::WaypointNavigator::getWaypointNames ( ) const

Definition at line 342 of file waypoints_navigator.cpp.

{
  return waypointsNames_;
}

References waypointsNames_.

Referenced by cl_nav2z::CpWaypointsVisualizer::onInitialize().

Here is the caller graph for this function:

◆ getWaypoints()

const std::vector< geometry_msgs::msg::Pose > & cl_nav2z::WaypointNavigator::getWaypoints ( ) const

Definition at line 321 of file waypoints_navigator.cpp.

{
  return waypoints_;
}

References waypoints_.

Referenced by cl_nav2z::CpWaypointsVisualizer::onInitialize().

Here is the caller graph for this function:

◆ insertWaypoint()

void cl_nav2z::WaypointNavigator::insertWaypoint	(	int	index,
		geometry_msgs::msg::Pose &	newpose
	)

private

Definition at line 280 of file waypoints_navigator.cpp.

{
  if (index >= 0 && index <= (int)waypoints_.size())
  {
    waypoints_.insert(waypoints_.begin(), index, newpose);
  }
}

References waypoints_.

◆ loadWayPointsFromFile()

void cl_nav2z::WaypointNavigator::loadWayPointsFromFile ( std::string filepath )

Definition at line 359 of file waypoints_navigator.cpp.

{
  RCLCPP_INFO_STREAM(getLogger(), "[WaypointNavigator] Loading file:" << filepath);
  this->waypoints_.clear();
  std::ifstream ifs(filepath.c_str(), std::ifstream::in);
  if (ifs.good() == false)
  {
    throw std::string("Waypoints file not found");
  }
 
  try
  {
#ifdef HAVE_NEW_YAMLCPP
    YAML::Node node = YAML::Load(ifs);
#else
    YAML::Parser parser(ifs);
    parser.GetNextDocument(node);
#endif
 
#ifdef HAVE_NEW_YAMLCPP
    const YAML::Node & wp_node_tmp = node["waypoints"];
    const YAML::Node * wp_node = wp_node_tmp ? &wp_node_tmp : NULL;
#else
    const YAML::Node * wp_node = node.FindValue("waypoints");
#endif
 
    if (wp_node != NULL)
    {
      for (uint64_t i = 0; i < wp_node->size(); ++i)
      {
        // Parse waypoint entries on YAML
        geometry_msgs::msg::Pose wp;
 
        try
        {
          // (*wp_node)[i]["name"] >> wp.name;
          // (*wp_node)[i]["frame_id"] >> wp.header.frame_id;
 
          auto wpnodei = (*wp_node)[i];
          wp.position.x = wpnodei["position"]["x"].as<double>();
          wp.position.y = wpnodei["position"]["y"].as<double>();
          wp.position.z = wpnodei["position"]["z"].as<double>();
          wp.orientation.x = wpnodei["orientation"]["x"].as<double>();
          wp.orientation.y = wpnodei["orientation"]["y"].as<double>();
          wp.orientation.z = wpnodei["orientation"]["z"].as<double>();
          wp.orientation.w = wpnodei["orientation"]["w"].as<double>();
 
          if (wpnodei["name"].IsDefined())
          {
            this->waypointsNames_.push_back(wpnodei["name"].as<std::string>());
          }
 
          this->waypoints_.push_back(wp);
        }
        catch (...)
        {
          RCLCPP_ERROR(getLogger(), "parsing waypoint file, syntax error in point %ld", i);
        }
      }
      RCLCPP_INFO_STREAM(getLogger(), "Parsed " << this->waypoints_.size() << " waypoints.");
    }
    else
    {
      RCLCPP_WARN_STREAM(getLogger(), "Couldn't find any waypoints in the provided yaml file.");
    }
  }
  catch (const YAML::ParserException & ex)
  {
    RCLCPP_ERROR_STREAM(
      getLogger(), "Error loading the Waypoints YAML file. Incorrect syntax: " << ex.what());
  }
}

References smacc2::ISmaccComponent::getLogger(), waypoints_, and waypointsNames_.

Here is the call graph for this function:

◆ loadWayPointsFromFile2()

void cl_nav2z::WaypointNavigator::loadWayPointsFromFile2 ( std::string filepath )

Definition at line 432 of file waypoints_navigator.cpp.

{
  RCLCPP_INFO_STREAM(getLogger(), "[WaypointNavigator] Loading file:" << filepath);
  this->waypoints_.clear();
  std::ifstream ifs(filepath.c_str(), std::ifstream::in);
  if (ifs.good() == false)
  {
    throw std::string("Waypoints file not found");
  }
 
  try
  {
#ifdef HAVE_NEW_YAMLCPP
    YAML::Node node = YAML::Load(ifs);
#else
    YAML::Parser parser(ifs);
    parser.GetNextDocument(node);
#endif
 
#ifdef HAVE_NEW_YAMLCPP
    const YAML::Node & wp_node_tmp = node["waypoints"];
    const YAML::Node * wp_node = wp_node_tmp ? &wp_node_tmp : NULL;
#else
    const YAML::Node * wp_node = node.FindValue("waypoints");
#endif
 
    if (wp_node != NULL)
    {
      for (uint64_t i = 0; i < wp_node->size(); ++i)
      {
        // Parse waypoint entries on YAML
        geometry_msgs::msg::Pose wp;
 
        try
        {
          // (*wp_node)[i]["name"] >> wp.name;
          // (*wp_node)[i]["frame_id"] >> wp.header.frame_id;
          wp.position.x = (*wp_node)[i]["x"].as<double>();
          wp.position.y = (*wp_node)[i]["y"].as<double>();
          auto name = (*wp_node)[i]["name"].as<std::string>();
 
          this->waypoints_.push_back(wp);
          this->waypointsNames_.push_back(name);
        }
        catch (...)
        {
          RCLCPP_ERROR(getLogger(), "parsing waypoint file, syntax error in point %ld", i);
        }
      }
      RCLCPP_INFO_STREAM(getLogger(), "Parsed " << this->waypoints_.size() << " waypoints.");
    }
    else
    {
      RCLCPP_WARN_STREAM(getLogger(), "Couldn't find any waypoints in the provided yaml file.");
    }
  }
  catch (const YAML::ParserException & ex)
  {
    RCLCPP_ERROR_STREAM(
      getLogger(), "Error loading the Waypoints YAML file. Incorrect syntax: " << ex.what());
  }
}

References smacc2::ISmaccComponent::getLogger(), waypoints_, and waypointsNames_.

Here is the call graph for this function:

◆ onGoalAborted()

void cl_nav2z::WaypointNavigator::onGoalAborted ( const ClNav2Z::WrappedResult & )

private

Definition at line 48 of file waypoints_navigator.cpp.

{
  stopWaitingResult();
 
  this->onNavigationRequestAborted();
}

References onNavigationRequestAborted, and stopWaitingResult().

Referenced by onNavigationResult().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ onGoalCancelled()

void cl_nav2z::WaypointNavigator::onGoalCancelled ( const ClNav2Z::WrappedResult & )

private

Definition at line 41 of file waypoints_navigator.cpp.

{
  stopWaitingResult();
 
  this->onNavigationRequestCancelled();
}

References onNavigationRequestCancelled, and stopWaitingResult().

Referenced by onNavigationResult().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ onGoalReached()

void cl_nav2z::WaypointNavigator::onGoalReached ( const ClNav2Z::WrappedResult & res )

private

Definition at line 55 of file waypoints_navigator.cpp.

{
  waypointsEventDispatcher.postWaypointEvent(currentWaypoint_);
  currentWaypoint_++;
  RCLCPP_WARN(
    getLogger(), "[WaypointNavigator] Goal result received, incrementing waypoint index: %ld",
    currentWaypoint_);
  stopWaitingResult();
 
  onNavigationRequestSucceded();
}

References currentWaypoint_, smacc2::ISmaccComponent::getLogger(), onNavigationRequestSucceded, cl_nav2z::WaypointEventDispatcher::postWaypointEvent(), stopWaitingResult(), and waypointsEventDispatcher.

Referenced by onNavigationResult().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ onInitialize()

void cl_nav2z::WaypointNavigator::onInitialize ( )

overridevirtual

Reimplemented from smacc2::ISmaccComponent.

Definition at line 39 of file waypoints_navigator.cpp.

39{ client_ = dynamic_cast<ClNav2Z *>(owner_); }

cl_nav2z::WaypointNavigator::client_

ClNav2Z * client_

Definition: waypoints_navigator.hpp:60

smacc2::ISmaccComponent::owner_

ISmaccClient * owner_

Definition: component.hpp:83

References client_, and smacc2::ISmaccComponent::owner_.

◆ onNavigationResult()

void cl_nav2z::WaypointNavigator::onNavigationResult ( const ClNav2Z::WrappedResult & r )

private

Definition at line 260 of file waypoints_navigator.cpp.

{
  if (r.code == rclcpp_action::ResultCode::SUCCEEDED)
  {
    this->onGoalReached(r);
  }
  else if (r.code == rclcpp_action::ResultCode::ABORTED)
  {
    this->onGoalAborted(r);
  }
  else if (r.code == rclcpp_action::ResultCode::CANCELED)
  {
    this->onGoalCancelled(r);
  }
  else
  {
    this->onGoalAborted(r);
  }
}

References onGoalAborted(), onGoalCancelled(), and onGoalReached().

Referenced by sendNextGoal().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ onOrthogonalAllocation()

template<typename TOrthogonal , typename TSourceObject >

void cl_nav2z::WaypointNavigator::onOrthogonalAllocation ( )

inline

Definition at line 67 of file waypoints_navigator.hpp.

  {
    waypointsEventDispatcher.initialize<TSourceObject, TOrthogonal>(client_);
  }

References client_, cl_nav2z::WaypointEventDispatcher::initialize(), and waypointsEventDispatcher.

Here is the call graph for this function:

◆ removeWaypoint()

void cl_nav2z::WaypointNavigator::removeWaypoint ( int index )

private

Definition at line 313 of file waypoints_navigator.cpp.

{
  if (index >= 0 && index < (int)waypoints_.size())
  {
    waypoints_.erase(waypoints_.begin() + index);
  }
}

References waypoints_.

◆ rewind()

void cl_nav2z::WaypointNavigator::rewind ( int count )

Definition at line 67 of file waypoints_navigator.cpp.

{
  currentWaypoint_--;
  if (currentWaypoint_ < 0) currentWaypoint_ = 0;
}

References currentWaypoint_.

◆ seekName()

void cl_nav2z::WaypointNavigator::seekName ( std::string name )

Definition at line 80 of file waypoints_navigator.cpp.

{
  bool found = false;
 
  auto previousWaypoint = currentWaypoint_;
 
  while (!found && currentWaypoint_ < (long)waypoints_.size())
  {
    auto & nextName = waypointsNames_[currentWaypoint_];
    RCLCPP_INFO(
      getLogger(), "[WaypointNavigator] seeking ,%ld/%ld candidate waypoint: %s", currentWaypoint_,
      waypoints_.size(), nextName.c_str());
    if (name == nextName)
    {
      found = true;
      RCLCPP_INFO(
        getLogger(), "[WaypointNavigator] found target waypoint: %s == %s-> found",
        nextName.c_str(), name.c_str());
    }
    else
    {
      RCLCPP_INFO(
        getLogger(), "[WaypointNavigator] current waypoint: %s != %s -> forward", nextName.c_str(),
        name.c_str());
      currentWaypoint_++;
    }
  }
 
  if (found)
  {
    if (currentWaypoint_ >= (long)waypoints_.size() - 1)
      currentWaypoint_ = (long)waypoints_.size() - 1;
  }
  else  // search backwards
  {
    currentWaypoint_ = previousWaypoint;
    while (!found && currentWaypoint_ > 0)
    {
      auto & nextName = waypointsNames_[currentWaypoint_];
      RCLCPP_INFO(
        getLogger(), "[WaypointNavigator] seeking , candidate waypoint: %s", nextName.c_str());
      if (name == nextName)
      {
        found = true;
        RCLCPP_INFO(
          getLogger(), "[WaypointNavigator] found target waypoint: %s == %s-> found",
          nextName.c_str(), name.c_str());
      }
      else
      {
        RCLCPP_INFO(
          getLogger(), "[WaypointNavigator] current waypoint: %s != %s -> rewind", nextName.c_str(),
          name.c_str());
        currentWaypoint_--;
      }
    }
  }
 
  RCLCPP_INFO(
    getLogger(), "[WaypointNavigator] seekName( %s), previous index: %ld, after index: %ld",
    name.c_str(), previousWaypoint, currentWaypoint_);
}

References currentWaypoint_, smacc2::ISmaccComponent::getLogger(), waypoints_, and waypointsNames_.

Referenced by cl_nav2z::CbNavigateNamedWaypoint::onEntry(), and cl_nav2z::CbSeekWaypoint::onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ sendNextGoal()

std::optional< std::shared_future< std::shared_ptr< rclcpp_action::ClientGoalHandle< nav2_msgs::action::NavigateToPose > > > > cl_nav2z::WaypointNavigator::sendNextGoal	(	std::optional< NavigateNextWaypointOptions >	options = `std::nullopt`,
		cl_nav2z::ClNav2Z::SmaccNavigateResultSignal::WeakPtr	callback = `cl_nav2z::ClNav2Z::SmaccNavigateResultSignal::WeakPtr()`
	)

Definition at line 155 of file waypoints_navigator.cpp.

{
  if (currentWaypoint_ >= 0 && currentWaypoint_ < (int)waypoints_.size())
  {
    auto & next = waypoints_[currentWaypoint_];
 
    std::string nextName;
    if ((long)waypointsNames_.size() > currentWaypoint_)
    {
      nextName = waypointsNames_[currentWaypoint_];
      RCLCPP_INFO(getLogger(), "[WaypointNavigator] sending goal, waypoint: %s", nextName.c_str());
    }
    else
    {
      RCLCPP_INFO(getLogger(), "[WaypointNavigator] sending goal, waypoint: %ld", currentWaypoint_);
    }
 
    ClNav2Z::Goal goal;
    auto p = client_->getComponent<cl_nav2z::Pose>();
    auto pose = p->toPoseMsg();
 
    // configuring goal
    goal.pose.header.frame_id = p->getReferenceFrame();
    //goal.pose.header.stamp = getNode()->now();
    goal.pose.pose = next;
 
    auto plannerSwitcher = client_->getComponent<PlannerSwitcher>();
    plannerSwitcher->setDefaultPlanners(false);
    if (options && options->controllerName_)
    {
      RCLCPP_WARN(
        getLogger(), "[WaypointsNavigator] override controller: %s",
        options->controllerName_->c_str());
 
      plannerSwitcher->setDesiredController(*options->controllerName_);
    }
    else
    {
      RCLCPP_WARN(getLogger(), "[WaypointsNavigator] Configuring default planners");
    }
 
    auto goalCheckerSwitcher = client_->getComponent<GoalCheckerSwitcher>();
 
    if (options && options->goalCheckerName_)
    {
      RCLCPP_WARN(
        getLogger(), "[WaypointsNavigator] override goal checker: %s",
        options->goalCheckerName_->c_str());
 
      goalCheckerSwitcher->setGoalCheckerId(*options->goalCheckerName_);
    }
    else
    {
      RCLCPP_WARN(getLogger(), "[WaypointsNavigator] Configuring default goal checker");
      goalCheckerSwitcher->setGoalCheckerId("goal_checker");
    }
 
    plannerSwitcher->commitPublish();
 
    // publish stuff
    // rclcpp::sleep_for(5s);
 
    RCLCPP_INFO(getLogger(), "[WaypointsNavigator] Getting odom tracker");
    auto odomTracker = client_->getComponent<cl_nav2z::odom_tracker::OdomTracker>();
    if (odomTracker != nullptr)
    {
      RCLCPP_INFO(getLogger(), "[WaypointsNavigator] Storing path in odom tracker");
 
      auto pathname = this->owner_->getStateMachine()->getCurrentState()->getName() + " - " +
                      getName() + " - " + nextName;
      odomTracker->pushPath(pathname);
      odomTracker->setStartPoint(pose);
      odomTracker->setWorkingMode(cl_nav2z::odom_tracker::WorkingMode::RECORD_PATH);
    }
 
    // SEND GOAL
    // if (!succeddedNav2ZClientConnection_.connected())
    // {
    //   this->succeddedNav2ZClientConnection_ =
    //     client_->onSucceeded(&WaypointNavigator::onGoalReached, this);
    //   this->cancelledNav2ZClientConnection_ =
    //     client_->onAborted(&WaypointNavigator::onGoalCancelled, this);
    //   this->abortedNav2ZClientConnection_ =
    //     client_->onCancelled(&WaypointNavigator::onGoalAborted, this);
    // }
 
    auto callbackptr = resultCallback.lock();
    succeddedNav2ZClientConnection_ = this->getStateMachine()->createSignalConnection(
      *callbackptr, &WaypointNavigator::onNavigationResult, this);
 
    return client_->sendGoal(goal, resultCallback);
    //return client_->sendGoal(goal);
  }
  else
  {
    RCLCPP_WARN(
      getLogger(),
      "[WaypointsNavigator] All waypoints were consumed. There is no more waypoints available.");
  }
 
  return std::nullopt;
}

References client_, smacc2::ISmaccStateMachine::createSignalConnection(), currentWaypoint_, smacc2::ISmaccClient::getComponent(), smacc2::ISmaccStateMachine::getCurrentState(), smacc2::ISmaccComponent::getLogger(), smacc2::ISmaccComponent::getName(), smacc2::ISmaccState::getName(), smacc2::ISmaccComponent::getStateMachine(), smacc2::ISmaccClient::getStateMachine(), onNavigationResult(), smacc2::ISmaccComponent::owner_, cl_nav2z::odom_tracker::RECORD_PATH, smacc2::client_bases::SmaccActionClientBase< ActionType >::sendGoal(), cl_nav2z::PlannerSwitcher::setDefaultPlanners(), succeddedNav2ZClientConnection_, cl_nav2z::Pose::toPoseMsg(), waypoints_, and waypointsNames_.

Referenced by cl_nav2z::CbNavigateNextWaypoint::onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setWaypoints() [1/2]

void cl_nav2z::WaypointNavigator::setWaypoints ( const std::vector< geometry_msgs::msg::Pose > & waypoints )

Definition at line 288 of file waypoints_navigator.cpp.

{
  this->waypoints_ = waypoints;
}

References waypoints_.

◆ setWaypoints() [2/2]

void cl_nav2z::WaypointNavigator::setWaypoints ( const std::vector< Pose2D > & waypoints )

Definition at line 293 of file waypoints_navigator.cpp.

{
  waypoints_.clear();
  waypointsNames_.clear();
  int i = 0;
  for (auto & p : waypoints)
  {
    geometry_msgs::msg::Pose pose;
    pose.position.x = p.x_;
    pose.position.y = p.y_;
    pose.position.z = 0.0;
    tf2::Quaternion q;
    q.setRPY(0, 0, p.yaw_);
    pose.orientation = tf2::toMsg(q);
 
    waypoints_.push_back(pose);
    waypointsNames_.push_back(std::to_string(i++));
  }
}

References waypoints_, and waypointsNames_.

◆ stopWaitingResult()

void cl_nav2z::WaypointNavigator::stopWaitingResult ( )

Definition at line 143 of file waypoints_navigator.cpp.

{
  if (succeddedNav2ZClientConnection_.connected())
  {
    this->succeddedNav2ZClientConnection_.disconnect();
    this->cancelledNav2ZClientConnection_.disconnect();
    this->abortedNav2ZClientConnection_.disconnect();
  }
}

References abortedNav2ZClientConnection_, cancelledNav2ZClientConnection_, and succeddedNav2ZClientConnection_.

Referenced by cl_nav2z::CbNavigateNextWaypoint::onExit(), onGoalAborted(), onGoalCancelled(), and onGoalReached().

Here is the caller graph for this function:

Member Data Documentation

◆ abortedNav2ZClientConnection_

boost::signals2::connection cl_nav2z::WaypointNavigator::abortedNav2ZClientConnection_

private

Definition at line 123 of file waypoints_navigator.hpp.

Referenced by stopWaitingResult().

◆ cancelledNav2ZClientConnection_

boost::signals2::connection cl_nav2z::WaypointNavigator::cancelledNav2ZClientConnection_

private

Definition at line 124 of file waypoints_navigator.hpp.

Referenced by stopWaitingResult().

◆ client_

ClNav2Z* cl_nav2z::WaypointNavigator::client_

Definition at line 60 of file waypoints_navigator.hpp.

Referenced by onInitialize(), onOrthogonalAllocation(), and sendNextGoal().

◆ currentWaypoint_

long cl_nav2z::WaypointNavigator::currentWaypoint_

Definition at line 96 of file waypoints_navigator.hpp.

Referenced by forward(), getCurrentWaypointIndex(), getCurrentWaypointName(), onGoalReached(), rewind(), seekName(), and sendNextGoal().

◆ onNavigationRequestAborted

smacc2::SmaccSignal<void()> cl_nav2z::WaypointNavigator::onNavigationRequestAborted

Definition at line 104 of file waypoints_navigator.hpp.

Referenced by onGoalAborted().

◆ onNavigationRequestCancelled

smacc2::SmaccSignal<void()> cl_nav2z::WaypointNavigator::onNavigationRequestCancelled

Definition at line 105 of file waypoints_navigator.hpp.

Referenced by onGoalCancelled().

◆ onNavigationRequestSucceded

smacc2::SmaccSignal<void()> cl_nav2z::WaypointNavigator::onNavigationRequestSucceded

Definition at line 103 of file waypoints_navigator.hpp.

Referenced by onGoalReached().

◆ succeddedNav2ZClientConnection_

boost::signals2::connection cl_nav2z::WaypointNavigator::succeddedNav2ZClientConnection_

private

Definition at line 122 of file waypoints_navigator.hpp.

Referenced by sendNextGoal(), and stopWaitingResult().

◆ waypoints_

std::vector<geometry_msgs::msg::Pose> cl_nav2z::WaypointNavigator::waypoints_

private

Definition at line 118 of file waypoints_navigator.hpp.

Referenced by forward(), getNamedPose(), getWaypoints(), insertWaypoint(), loadWayPointsFromFile(), loadWayPointsFromFile2(), removeWaypoint(), seekName(), sendNextGoal(), and setWaypoints().

◆ waypointsEventDispatcher

WaypointEventDispatcher cl_nav2z::WaypointNavigator::waypointsEventDispatcher

Definition at line 58 of file waypoints_navigator.hpp.

Referenced by onGoalReached(), and onOrthogonalAllocation().

◆ waypointsNames_

std::vector<std::string> cl_nav2z::WaypointNavigator::waypointsNames_

private

Definition at line 120 of file waypoints_navigator.hpp.

Referenced by getCurrentWaypointName(), getNamedPose(), getWaypointNames(), loadWayPointsFromFile(), loadWayPointsFromFile2(), seekName(), sendNextGoal(), and setWaypoints().

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

smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/waypoints_navigator.hpp
smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/waypoints_navigator.cpp

Public Member Functions

Public Attributes

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ WaypointNavigator()

Member Function Documentation

◆ forward()

◆ getCurrentWaypointIndex()

◆ getCurrentWaypointName()

◆ getNamedPose()

◆ getWaypointNames()

◆ getWaypoints()

◆ insertWaypoint()

◆ loadWayPointsFromFile()

◆ loadWayPointsFromFile2()

◆ onGoalAborted()

◆ onGoalCancelled()

◆ onGoalReached()

◆ onInitialize()

◆ onNavigationResult()

◆ onOrthogonalAllocation()

◆ removeWaypoint()

◆ rewind()

◆ seekName()

◆ sendNextGoal()

◆ setWaypoints() [1/2]

◆ setWaypoints() [2/2]

◆ stopWaitingResult()

Member Data Documentation

◆ abortedNav2ZClientConnection_

◆ cancelledNav2ZClientConnection_

◆ client_

◆ currentWaypoint_

◆ onNavigationRequestAborted

◆ onNavigationRequestCancelled

◆ onNavigationRequestSucceded

◆ succeddedNav2ZClientConnection_

◆ waypoints_

◆ waypointsEventDispatcher

◆ waypointsNames_