#include <waypoints_navigator.h>

Inheritance diagram for cl_move_base_z::WaypointNavigator:

Collaboration diagram for cl_move_base_z::WaypointNavigator:

Public Member Functions
	WaypointNavigator ()

virtual void	onInitialize () override

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

void	removeWaypoint (int index)

void	loadWayPointsFromFile (std::string filepath)

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

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

void	sendNextGoal ()

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

long	getCurrentWaypointIndex () const

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

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

virtual	~ISmaccComponent ()

virtual std::string	getName () const

Public Attributes
WaypointEventDispatcher	waypointsEventDispatcher

ClMoveBaseZ *	client_

int	currentWaypoint_

Private Member Functions
void	onGoalReached (ClMoveBaseZ::ResultConstPtr &res)

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

boost::signals2::connection	succeddedConnection_

Additional Inherited Members
Protected Member Functions inherited from smacc::ISmaccComponent
virtual void	initialize (ISmaccClient *owner)

void	setStateMachine (ISmaccStateMachine *stateMachine)

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)

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

virtual void	onInitialize ()

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)

Protected Attributes inherited from smacc::ISmaccComponent
ISmaccStateMachine *	stateMachine_

ISmaccClient *	owner_

Detailed Description

Definition at line 32 of file waypoints_navigator.h.

Constructor & Destructor Documentation

◆ WaypointNavigator()

cl_move_base_z::WaypointNavigator::WaypointNavigator ( )

Definition at line 14 of file waypoints_navigator.cpp.

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

Member Function Documentation

◆ getCurrentWaypointIndex()

long cl_move_base_z::WaypointNavigator::getCurrentWaypointIndex ( ) const

Definition at line 110 of file waypoints_navigator.cpp.

{
  return currentWaypoint_;
}

References currentWaypoint_.

◆ getWaypoints()

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

Definition at line 105 of file waypoints_navigator.cpp.

{
  return waypoints_;
}

References waypoints_.

◆ insertWaypoint()

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

Definition at line 69 of file waypoints_navigator.cpp.

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

References waypoints_.

◆ loadWayPointsFromFile()

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

Definition at line 116 of file waypoints_navigator.cpp.

{
  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::Pose wp;
 
        try
        {
          // (*wp_node)[i]["name"] >> wp.name;
          // (*wp_node)[i]["frame_id"] >> wp.header.frame_id;
          wp.position.x = (*wp_node)[i]["position"]["x"].as<double>();
          wp.position.y = (*wp_node)[i]["position"]["y"].as<double>();
          wp.position.z = (*wp_node)[i]["position"]["z"].as<double>();
          wp.orientation.x = (*wp_node)[i]["orientation"]["x"].as<double>();
          wp.orientation.y = (*wp_node)[i]["orientation"]["y"].as<double>();
          wp.orientation.z = (*wp_node)[i]["orientation"]["z"].as<double>();
          wp.orientation.w = (*wp_node)[i]["orientation"]["w"].as<double>();
 
          this->waypoints_.push_back(wp);
        }
        catch (...)
        {
          ROS_ERROR("parsing waypoint file, syntax error in point %d", i);
        }
      }
      ROS_INFO_STREAM("Parsed " << this->waypoints_.size() << " waypoints.");
    }
    else
    {
      ROS_WARN_STREAM("Couldn't find any waypoints in the provided yaml file.");
    }
  }
  catch (const YAML::ParserException &ex)
  {
    ROS_ERROR_STREAM("Error loading the Waypoints YAML file. Incorrect syntax: " << ex.what());
  }
}

References waypoints_.

◆ onGoalReached()

void cl_move_base_z::WaypointNavigator::onGoalReached ( ClMoveBaseZ::ResultConstPtr & res )

private

Definition at line 25 of file waypoints_navigator.cpp.

{
  waypointsEventDispatcher.postWaypointEvent(currentWaypoint_);
  currentWaypoint_++;
  this->succeddedConnection_.disconnect();
}

References currentWaypoint_, cl_move_base_z::WaypointEventDispatcher::postWaypointEvent(), succeddedConnection_, and waypointsEventDispatcher.

Referenced by sendNextGoal().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ onInitialize()

void cl_move_base_z::WaypointNavigator::onInitialize ( )

overridevirtual

Reimplemented from smacc::ISmaccComponent.

Definition at line 20 of file waypoints_navigator.cpp.

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

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

◆ onOrthogonalAllocation()

template<typename TOrthogonal , typename TSourceObject >

void cl_move_base_z::WaypointNavigator::onOrthogonalAllocation ( )

inline

Definition at line 60 of file waypoints_navigator.h.

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

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

Here is the call graph for this function:

◆ removeWaypoint()

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

Definition at line 97 of file waypoints_navigator.cpp.

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

References waypoints_.

◆ sendNextGoal()

void cl_move_base_z::WaypointNavigator::sendNextGoal ( )

Definition at line 32 of file waypoints_navigator.cpp.

{
  if (currentWaypoint_ >= 0 && currentWaypoint_ < waypoints_.size())
  {
    auto &next = waypoints_[currentWaypoint_];
 
    auto odomTracker = client_->getComponent<cl_move_base_z::odom_tracker::OdomTracker>();
    auto p = client_->getComponent<cl_move_base_z::Pose>();
    auto pose = p->toPoseMsg();
 
    ClMoveBaseZ::Goal goal;
    goal.target_pose.header.frame_id = p->getReferenceFrame();
    goal.target_pose.header.stamp = ros::Time::now();
    goal.target_pose.pose = next;
 
    auto plannerSwitcher = client_->getComponent<PlannerSwitcher>();
    plannerSwitcher->setDefaultPlanners();
 
    ros::spinOnce();
    ros::Duration(5).sleep();
 
    if (odomTracker != nullptr)
    {
      odomTracker->pushPath("FreeNavigationToGoalWaypointPose");
      odomTracker->setStartPoint(pose);
      odomTracker->setWorkingMode(cl_move_base_z::odom_tracker::WorkingMode::RECORD_PATH);
    }
 
    this->succeddedConnection_ = client_->onSucceeded(&WaypointNavigator::onGoalReached, this);
    client_->sendGoal(goal);
  }
  else
  {
    ROS_WARN("[WaypointsNavigator] All waypoints were consumed. There is no more waypoints available.");
  }
}

References client_, currentWaypoint_, smacc::ISmaccClient::getComponent(), onGoalReached(), smacc::client_bases::SmaccActionClientBase< ActionType >::onSucceeded(), cl_move_base_z::odom_tracker::RECORD_PATH, smacc::client_bases::SmaccActionClientBase< ActionType >::sendGoal(), cl_move_base_z::PlannerSwitcher::setDefaultPlanners(), succeddedConnection_, and waypoints_.

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

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setWaypoints() [1/2]

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

Definition at line 77 of file waypoints_navigator.cpp.

{
  this->waypoints_ = waypoints;
}

References waypoints_.

◆ setWaypoints() [2/2]

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

Definition at line 82 of file waypoints_navigator.cpp.

{
  this->waypoints_.clear();
  for (auto &p : waypoints)
  {
    geometry_msgs::Pose pose;
    pose.position.x = p.x_;
    pose.position.y = p.y_;
    pose.position.z = 0.0;
    pose.orientation = tf::createQuaternionMsgFromYaw(p.yaw_);
 
    this->waypoints_.push_back(pose);
  }
}

References waypoints_.

Member Data Documentation

◆ client_

ClMoveBaseZ* cl_move_base_z::WaypointNavigator::client_

Definition at line 37 of file waypoints_navigator.h.

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

◆ currentWaypoint_

int cl_move_base_z::WaypointNavigator::currentWaypoint_

Definition at line 65 of file waypoints_navigator.h.

Referenced by getCurrentWaypointIndex(), onGoalReached(), and sendNextGoal().

◆ succeddedConnection_

boost::signals2::connection cl_move_base_z::WaypointNavigator::succeddedConnection_

private

Definition at line 72 of file waypoints_navigator.h.

Referenced by onGoalReached(), and sendNextGoal().

◆ waypoints_

std::vector<geometry_msgs::Pose> cl_move_base_z::WaypointNavigator::waypoints_

private

Definition at line 70 of file waypoints_navigator.h.

Referenced by getWaypoints(), insertWaypoint(), loadWayPointsFromFile(), removeWaypoint(), sendNextGoal(), and setWaypoints().

◆ waypointsEventDispatcher

WaypointEventDispatcher cl_move_base_z::WaypointNavigator::waypointsEventDispatcher

Definition at line 35 of file waypoints_navigator.h.

Referenced by onGoalReached(), and onOrthogonalAllocation().

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

smacc_client_library/move_base_z_client/move_base_z_client_plugin/include/move_base_z_client_plugin/components/waypoints_navigator/waypoints_navigator.h
smacc_client_library/move_base_z_client/move_base_z_client_plugin/src/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

◆ getCurrentWaypointIndex()

◆ getWaypoints()

◆ insertWaypoint()

◆ loadWayPointsFromFile()

◆ onGoalReached()

◆ onInitialize()

◆ onOrthogonalAllocation()

◆ removeWaypoint()

◆ sendNextGoal()

◆ setWaypoints() [1/2]

◆ setWaypoints() [2/2]

Member Data Documentation

◆ client_

◆ currentWaypoint_

◆ succeddedConnection_

◆ waypoints_

◆ waypointsEventDispatcher