cl_nav2z::CbUndoPathBackwards Class Reference

#include <cb_undo_path_backwards.hpp>

Inheritance diagram for cl_nav2z::CbUndoPathBackwards:

Collaboration diagram for cl_nav2z::CbUndoPathBackwards:

Public Member Functions
	CbUndoPathBackwards (std::optional< CbUndoPathBackwardsOptions > options=std::nullopt)
void	onEntry () override
void	onExit () override
Public Member Functions inherited from cl_nav2z::CbNav2ZClientBehaviorBase
virtual	~CbNav2ZClientBehaviorBase ()
template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()
Public Member Functions inherited from smacc2::SmaccAsyncClientBehavior
template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()
virtual	~SmaccAsyncClientBehavior ()
template<typename TCallback , typename T >
boost::signals2::connection	onSuccess (TCallback callback, T *object)
template<typename TCallback , typename T >
boost::signals2::connection	onFinished (TCallback callback, T *object)
template<typename TCallback , typename T >
boost::signals2::connection	onFailure (TCallback callback, T *object)
void	requestForceFinish ()
void	executeOnEntry () override
void	executeOnExit () override
void	waitOnEntryThread (bool requestFinish)
template<typename TCallbackMethod , typename T >
boost::signals2::connection	onSuccess (TCallbackMethod callback, T *object)
template<typename TCallbackMethod , typename T >
boost::signals2::connection	onFinished (TCallbackMethod callback, T *object)
template<typename TCallbackMethod , typename T >
boost::signals2::connection	onFailure (TCallbackMethod callback, T *object)
Public Member Functions inherited from smacc2::ISmaccClientBehavior
	ISmaccClientBehavior ()
virtual	~ISmaccClientBehavior ()
ISmaccStateMachine *	getStateMachine ()
std::string	getName () const
template<typename SmaccClientType >
void	requiresClient (SmaccClientType *&storage)
template<typename SmaccComponentType >
void	requiresComponent (SmaccComponentType *&storage, bool throwExceptionIfNotExist=false)
virtual void	onEntry ()
virtual void	onExit ()
virtual void	executeOnEntry ()
virtual void	executeOnExit ()

Private Attributes
std::shared_ptr< tf2_ros::Buffer >	listener
OdomTracker *	odomTracker
std::optional< CbUndoPathBackwardsOptions >	options_

Additional Inherited Members
Protected Member Functions inherited from cl_nav2z::CbNav2ZClientBehaviorBase
void	sendGoal (ClNav2Z::Goal &goal)
void	cancelGoal ()
bool	isOwnActionResponse (const ClNav2Z::WrappedResult &)
virtual void	onNavigationResult (const ClNav2Z::WrappedResult &)
virtual void	onNavigationActionSuccess (const ClNav2Z::WrappedResult &)
virtual void	onNavigationActionAbort (const ClNav2Z::WrappedResult &)
Protected Member Functions inherited from smacc2::SmaccAsyncClientBehavior
void	postSuccessEvent ()
void	postFailureEvent ()
virtual void	dispose () override
bool	isShutdownRequested ()
Protected Member Functions inherited from smacc2::ISmaccClientBehavior
virtual void	runtimeConfigure ()
template<typename EventType >
void	postEvent (const EventType &ev)
template<typename EventType >
void	postEvent ()
ISmaccState *	getCurrentState ()
virtual void	dispose ()
virtual rclcpp::Node::SharedPtr	getNode () const
virtual rclcpp::Logger	getLogger () const
Protected Attributes inherited from cl_nav2z::CbNav2ZClientBehaviorBase
cl_nav2z::ClNav2Z *	nav2zClient_
cl_nav2z::ClNav2Z::SmaccNavigateResultSignal::SharedPtr	navigationCallback_
rclcpp_action::ResultCode	navigationResult_
std::shared_future< std::shared_ptr< rclcpp_action::ClientGoalHandle< nav2_msgs::action::NavigateToPose > > >	goalHandleFuture_

Detailed Description

Definition at line 40 of file cb_undo_path_backwards.hpp.

Constructor & Destructor Documentation

CbUndoPathBackwards()

cl_nav2z::CbUndoPathBackwards::CbUndoPathBackwards

(

std::optional< CbUndoPathBackwardsOptions >

options = std::nullopt

)

Definition at line 33 of file cb_undo_path_backwards.cpp.

{
  options_ = options;
}

References options_.

Member Function Documentation

onEntry()

void cl_nav2z::CbUndoPathBackwards::onEntry ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 38 of file cb_undo_path_backwards.cpp.

{
  listener = std::make_shared<tf2_ros::Buffer>(this->getNode()->get_clock());
  odomTracker = nav2zClient_->getComponent<OdomTracker>();
 
  odomTracker->logStateString(false);
 
  auto plannerSwitcher = nav2zClient_->getComponent<PlannerSwitcher>();
 
  nav_msgs::msg::Path forwardpath = odomTracker->getPath();
  // RCLCPP_INFO_STREAM(getLogger(),"[UndoPathBackward] Current path backwards: " << forwardpath);
 
  odomTracker->setWorkingMode(WorkingMode::CLEAR_PATH);
 
  ClNav2Z::Goal goal;
 
  auto goalCheckerSwitcher = nav2zClient_->getComponent<GoalCheckerSwitcher>();
 
  if (options_ && options_->goalCheckerId_)
  {
    goalCheckerSwitcher->setGoalCheckerId(*options_->goalCheckerId_);
  }
  else
  {
    goalCheckerSwitcher->setGoalCheckerId("undo_path_backwards_goal_checker");
  }
 
  // WARNING: There might be some race condition with the remote undo global planner were the global path was not
  // received yet
  // TODO: waiting notification from global planner that it is loaded
  rclcpp::sleep_for(1s);
 
  // this line is used to flush/reset backward planner in the case it were already there
  // plannerSwitcher->setDefaultPlanners();
  if (forwardpath.poses.size() > 0)
  {
    goal.pose = forwardpath.poses.front();
    //goal.pose.header.stamp = getNode()->now();
    goal.pose.header.stamp = rclcpp::Time(0);
 
    if (options_ && options_->undoControllerName_)
    {
      plannerSwitcher->setUndoPathBackwardPlanner(false);
      RCLCPP_INFO_STREAM(
        getLogger(),
        "[" << getName() << "] Undoing path with controller: " << *options_->undoControllerName_);
      plannerSwitcher->setDesiredController(*options_->undoControllerName_);
      plannerSwitcher->commitPublish();
    }
    else
    {
      plannerSwitcher->setUndoPathBackwardPlanner();
    }
 
    this->sendGoal(goal);
  }
}

References smacc2::ISmaccClient::getComponent(), smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), smacc2::ISmaccClientBehavior::getNode(), cl_nav2z::odom_tracker::OdomTracker::getPath(), listener, cl_nav2z::odom_tracker::OdomTracker::logStateString(), cl_nav2z::CbNav2ZClientBehaviorBase::nav2zClient_, odomTracker, options_, cl_nav2z::CbNav2ZClientBehaviorBase::sendGoal(), cl_nav2z::GoalCheckerSwitcher::setGoalCheckerId(), and cl_nav2z::odom_tracker::OdomTracker::setWorkingMode().

Here is the call graph for this function:

onExit()

void cl_nav2z::CbUndoPathBackwards::onExit ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 96 of file cb_undo_path_backwards.cpp.

{
  RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Exiting: undo navigation ");
 
  if (this->navigationResult_ == rclcpp_action::ResultCode::SUCCEEDED)
  {
    RCLCPP_INFO_STREAM(
      getLogger(), getName() << " - [CbUndoPathBackwards] Exiting: undo navigation successful, "
                                "popping odom tracker path");
    odomTracker = nav2zClient_->getComponent<OdomTracker>();
    odomTracker->popPath();
 
    odomTracker->logStateString(false);
  }
  else
  {
    RCLCPP_INFO_STREAM(
      getLogger(), getName() << " - [CbUndoPathBackwards] Exiting: undo navigation abort, avoiding "
                                "popping current path");
 
    odomTracker = nav2zClient_->getComponent<OdomTracker>();
    odomTracker->logStateString(false);
    // navigation interrupted or aborted. The path may be not totally undone.
    // We keep the odom tracker in its current state, probably in the middle of the undoing process.
    // Could you try to repeat the behavior?
  }
}

References smacc2::ISmaccClient::getComponent(), smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), cl_nav2z::odom_tracker::OdomTracker::logStateString(), cl_nav2z::CbNav2ZClientBehaviorBase::nav2zClient_, cl_nav2z::CbNav2ZClientBehaviorBase::navigationResult_, odomTracker, and cl_nav2z::odom_tracker::OdomTracker::popPath().

Here is the call graph for this function:

Member Data Documentation

listener

std::shared_ptr<tf2_ros::Buffer> cl_nav2z::CbUndoPathBackwards::listener

private

Definition at line 50 of file cb_undo_path_backwards.hpp.

Referenced by onEntry().

odomTracker

OdomTracker* cl_nav2z::CbUndoPathBackwards::odomTracker

private

Definition at line 52 of file cb_undo_path_backwards.hpp.

Referenced by onEntry(), and onExit().

options_

std::optional<CbUndoPathBackwardsOptions> cl_nav2z::CbUndoPathBackwards::options_

private

Definition at line 54 of file cb_undo_path_backwards.hpp.

Referenced by CbUndoPathBackwards(), and onEntry().

---

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

• smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_undo_path_backwards.hpp
• smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_undo_path_backwards.cpp