cl_nav2z::CpWaypointNavigatorBase Class Reference

#include <cp_waypoints_navigator_base.hpp>

Inheritance diagram for cl_nav2z::CpWaypointNavigatorBase:

Collaboration diagram for cl_nav2z::CpWaypointNavigatorBase:

Public Member Functions
	CpWaypointNavigatorBase ()
virtual	~CpWaypointNavigatorBase ()
void	onInitialize () override
template<typename TOrthogonal , typename TSourceObject >
void	onStateOrthogonalAllocation ()
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)
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
geometry_msgs::msg::Pose	getPose (int index) const
geometry_msgs::msg::Pose	getCurrentPose () const
long	getCurrentWaypointIndex () const
std::optional< std::string >	getCurrentWaypointName () const
void	rewind (int count)
void	forward (int count)
void	seekName (std::string name)
void	loadWaypointsFromYamlParameter (std::string parameter_name, std::string yaml_file_package_name)
void	notifyGoalReached ()
Public Member Functions inherited from smacc2::ISmaccComponent
	ISmaccComponent ()
virtual	~ISmaccComponent ()
virtual std::string	getName () const

Public Attributes
WaypointEventDispatcher	waypointsEventDispatcher
long	currentWaypoint_

Protected Member Functions
void	insertWaypoint (int index, geometry_msgs::msg::Pose &newpose)
void	removeWaypoint (int index)
Protected Member Functions inherited from smacc2::ISmaccComponent
template<typename TOrthogonal , typename TClient >
void	onComponentInitialization ()
template<typename EventType >
void	postEvent (const EventType &ev)
template<typename EventType >
void	postEvent ()
template<typename TOrthogonal , typename TSourceObject >
void	onStateOrthogonalAllocation ()
template<typename TComponent >
void	requiresComponent (TComponent *&requiredComponentStorage, ComponentRequirement requirementType=ComponentRequirement::SOFT)
template<typename TComponent >
void	requiresComponent (std::string name, TComponent *&requiredComponentStorage, ComponentRequirement requirementType=ComponentRequirement::SOFT)
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
std::vector< geometry_msgs::msg::Pose >	waypoints_
std::vector< std::string >	waypointsNames_
Protected Attributes inherited from smacc2::ISmaccComponent
ISmaccStateMachine *	stateMachine_
ISmaccClient *	owner_

Detailed Description

Definition at line 47 of file cp_waypoints_navigator_base.hpp.

Constructor & Destructor Documentation

CpWaypointNavigatorBase()

cl_nav2z::CpWaypointNavigatorBase::CpWaypointNavigatorBase

(

)

Definition at line 40 of file cp_waypoints_navigator.cpp.

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

~CpWaypointNavigatorBase()

cl_nav2z::CpWaypointNavigatorBase::~CpWaypointNavigatorBase ( )

virtual

Definition at line 42 of file cp_waypoints_navigator.cpp.

{}

Member Function Documentation

forward()

void cl_nav2z::CpWaypointNavigatorBase::forward

(

int

count

)

Definition at line 90 of file cp_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(), and cl_nav2z::CbNavigateNextWaypointFree::onSucessCallback().

Here is the caller graph for this function:

getCurrentPose()

geometry_msgs::msg::Pose cl_nav2z::CpWaypointNavigatorBase::getCurrentPose

(

)

const

Definition at line 391 of file cp_waypoints_navigator.cpp.

{
  if (currentWaypoint_ >= 0 && currentWaypoint_ < (int)waypoints_.size())
  {
    return waypoints_[currentWaypoint_];
  }
  else
  {
    throw std::out_of_range("Waypoint index out of range");
  }
}

References currentWaypoint_, and waypoints_.

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

Here is the caller graph for this function:

getCurrentWaypointIndex()

long cl_nav2z::CpWaypointNavigatorBase::getCurrentWaypointIndex

(

)

const

Definition at line 434 of file cp_waypoints_navigator.cpp.

{ return currentWaypoint_; }

References currentWaypoint_.

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

Here is the caller graph for this function:

getCurrentWaypointName()

std::optional< std::string > cl_nav2z::CpWaypointNavigatorBase::getCurrentWaypointName

(

)

const

Definition at line 425 of file cp_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::CpWaypointNavigatorBase::getNamedPose

(

std::string

name

)

const

Definition at line 403 of file cp_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_.

getPose()

geometry_msgs::msg::Pose cl_nav2z::CpWaypointNavigatorBase::getPose

(

int

index

)

const

Definition at line 380 of file cp_waypoints_navigator.cpp.

{
  if (index >= 0 && index < (int)waypoints_.size())
  {
    return waypoints_[index];
  }
  else
  {
    throw std::out_of_range("Waypoint index out of range");
  }
}

References waypoints_.

getWaypointNames()

const std::vector< std::string > & cl_nav2z::CpWaypointNavigatorBase::getWaypointNames

(

)

const

Definition at line 420 of file cp_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::CpWaypointNavigatorBase::getWaypoints

(

)

const

Definition at line 375 of file cp_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::CpWaypointNavigatorBase::insertWaypoint ( int index, geometry_msgs::msg::Pose & newpose )

protected

Definition at line 334 of file cp_waypoints_navigator.cpp.

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

References waypoints_.

loadWayPointsFromFile()

void cl_nav2z::CpWaypointNavigatorBase::loadWayPointsFromFile

(

std::string

filepath

)

Definition at line 437 of file cp_waypoints_navigator.cpp.

{
  RCLCPP_INFO_STREAM(getLogger(), "[CpWaypointNavigatorBase] 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 (std::size_t i = 0; i < wp_node->size(); ++i)
      {
        // Parse waypoint entries on YAML
        geometry_msgs::msg::Pose wp;
 
        try
        {
          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_.

Referenced by loadWaypointsFromYamlParameter(), and cl_nav2z::CbLoadWaypointsFile::onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

loadWayPointsFromFile2()

void cl_nav2z::CpWaypointNavigatorBase::loadWayPointsFromFile2

(

std::string

filepath

)

Definition at line 507 of file cp_waypoints_navigator.cpp.

{
  RCLCPP_INFO_STREAM(getLogger(), "[CpWaypointNavigator] 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 (std::size_t i = 0; i < wp_node->size(); ++i)
      {
        // Parse waypoint entries on YAML
        geometry_msgs::msg::Pose wp;
 
        try
        {
          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:

loadWaypointsFromYamlParameter()

void cl_nav2z::CpWaypointNavigatorBase::loadWaypointsFromYamlParameter	(	std::string	parameter_name,
		std::string	yaml_file_package_name )

Definition at line 160 of file cp_waypoints_navigator.cpp.

{
  // if it is the first time and the waypoints navigator is not configured
  std::string planfilepath;
  planfilepath = getNode()->declare_parameter(parameter_name, planfilepath);
  RCLCPP_INFO(getLogger(), "waypoints plan parameter: %s", planfilepath.c_str());
  if (getNode()->get_parameter(parameter_name, planfilepath))
  {
    std::string package_share_directory =
      ament_index_cpp::get_package_share_directory(yaml_file_package_name);
 
    RCLCPP_INFO(getLogger(), "file macro path: %s", planfilepath.c_str());
 
    boost::replace_all(planfilepath, "$(pkg_share)", package_share_directory);
 
    RCLCPP_INFO(getLogger(), "package share path: %s", package_share_directory.c_str());
    RCLCPP_INFO(getLogger(), "waypoints plan file: %s", planfilepath.c_str());
 
    this->loadWayPointsFromFile(planfilepath);
    RCLCPP_INFO(getLogger(), "waypoints plan: %s", planfilepath.c_str());
  }
  else
  {
    RCLCPP_ERROR(getLogger(), "waypoints plan file not found: NONE");
  }
}

References smacc2::ISmaccComponent::getLogger(), smacc2::ISmaccComponent::getNode(), and loadWayPointsFromFile().

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

Here is the call graph for this function:

Here is the caller graph for this function:

notifyGoalReached()

void cl_nav2z::CpWaypointNavigatorBase::notifyGoalReached

(

)

Definition at line 303 of file cp_waypoints_navigator.cpp.

{
  // when it is the last waypoint post an finalization EOF event
  if (currentWaypoint_ == (long)waypoints_.size() - 1)
  {
    RCLCPP_WARN(getLogger(), "[CpWaypointNavigator] Last waypoint reached, posting EOF event. ");
    this->postEvent<EvWaypointFinal>();
  }
}

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

Referenced by cl_nav2z::CpWaypointNavigator::onGoalReached(), and cl_nav2z::CbNavigateNextWaypointFree::onSucessCallback().

Here is the call graph for this function:

Here is the caller graph for this function:

onInitialize()

void cl_nav2z::CpWaypointNavigatorBase::onInitialize ( )

overridevirtual

Reimplemented from smacc2::ISmaccComponent.

Definition at line 46 of file cp_waypoints_navigator.cpp.

{}

onStateOrthogonalAllocation()

template<typename TOrthogonal , typename TSourceObject >

void cl_nav2z::CpWaypointNavigatorBase::onStateOrthogonalAllocation ( )

inline

Definition at line 59 of file cp_waypoints_navigator_base.hpp.

  {
    ClNav2Z * client = dynamic_cast<ClNav2Z *>(owner_);
    waypointsEventDispatcher.initialize<TSourceObject, TOrthogonal>(client);
  }

References WaypointEventDispatcher::initialize(), smacc2::ISmaccComponent::owner_, and waypointsEventDispatcher.

Here is the call graph for this function:

removeWaypoint()

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

protected

Definition at line 367 of file cp_waypoints_navigator.cpp.

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

References waypoints_.

rewind()

void cl_nav2z::CpWaypointNavigatorBase::rewind

(

int

count

)

Definition at line 84 of file cp_waypoints_navigator.cpp.

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

References currentWaypoint_.

seekName()

void cl_nav2z::CpWaypointNavigatorBase::seekName

(

std::string

name

)

Definition at line 97 of file cp_waypoints_navigator.cpp.

{
  bool found = false;
 
  auto previousWaypoint = currentWaypoint_;
 
  while (!found && currentWaypoint_ < (long)waypoints_.size())
  {
    auto & nextName = waypointsNames_[currentWaypoint_];
    RCLCPP_INFO(
      getLogger(), "[CpWaypointNavigator] seeking ,%ld/%ld candidate waypoint: %s",
      currentWaypoint_, waypoints_.size(), nextName.c_str());
    if (name == nextName)
    {
      found = true;
      RCLCPP_INFO(
        getLogger(), "[CpWaypointNavigator] found target waypoint: %s == %s-> found",
        nextName.c_str(), name.c_str());
    }
    else
    {
      RCLCPP_INFO(
        getLogger(), "[CpWaypointNavigator] 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(), "[CpWaypointNavigator] seeking , candidate waypoint: %s", nextName.c_str());
      if (name == nextName)
      {
        found = true;
        RCLCPP_INFO(
          getLogger(), "[CpWaypointNavigator] found target waypoint: %s == %s-> found",
          nextName.c_str(), name.c_str());
      }
      else
      {
        RCLCPP_INFO(
          getLogger(), "[CpWaypointNavigator] current waypoint: %s != %s -> rewind",
          nextName.c_str(), name.c_str());
        currentWaypoint_--;
      }
    }
  }
 
  RCLCPP_INFO(
    getLogger(), "[CpWaypointNavigator] 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:

setWaypoints() [1/2]

void cl_nav2z::CpWaypointNavigatorBase::setWaypoints

(

const std::vector< geometry_msgs::msg::Pose > &

waypoints

)

Definition at line 342 of file cp_waypoints_navigator.cpp.

{
  this->waypoints_ = waypoints;
}

References waypoints_.

setWaypoints() [2/2]

void cl_nav2z::CpWaypointNavigatorBase::setWaypoints

(

const std::vector< Pose2D > &

waypoints

)

Definition at line 347 of file cp_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_.

Member Data Documentation

currentWaypoint_

long cl_nav2z::CpWaypointNavigatorBase::currentWaypoint_

Definition at line 82 of file cp_waypoints_navigator_base.hpp.

Referenced by forward(), getCurrentPose(), getCurrentWaypointIndex(), getCurrentWaypointName(), notifyGoalReached(), cl_nav2z::CbLoadWaypointsFile::onEntry(), cl_nav2z::CpWaypointNavigator::onGoalReached(), rewind(), seekName(), and cl_nav2z::CpWaypointNavigator::sendNextGoal().

waypoints_

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

protected

Definition at line 99 of file cp_waypoints_navigator_base.hpp.

Referenced by forward(), getCurrentPose(), getNamedPose(), getPose(), getWaypoints(), insertWaypoint(), loadWayPointsFromFile(), loadWayPointsFromFile2(), notifyGoalReached(), removeWaypoint(), seekName(), cl_nav2z::CpWaypointNavigator::sendNextGoal(), setWaypoints(), and setWaypoints().

waypointsEventDispatcher

WaypointEventDispatcher cl_nav2z::CpWaypointNavigatorBase::waypointsEventDispatcher

Definition at line 50 of file cp_waypoints_navigator_base.hpp.

Referenced by cl_nav2z::CpWaypointNavigator::onGoalReached(), cl_nav2z::CpWaypointNavigator::onStateOrthogonalAllocation(), and onStateOrthogonalAllocation().

waypointsNames_

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

protected

Definition at line 101 of file cp_waypoints_navigator_base.hpp.

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

---

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

• smacc2_client_library/cl_nav2z/cl_nav2z/include/cl_nav2z/components/waypoints_navigator/cp_waypoints_navigator_base.hpp
• smacc2_client_library/cl_nav2z/cl_nav2z/src/cl_nav2z/components/waypoints_navigator/cp_waypoints_navigator.cpp