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
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)
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)

Public Attributes
std::optional< std::string >	goalChecker_

Private Attributes
std::shared_ptr< tf2_ros::Buffer >	listener

Additional Inherited Members
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 ()
virtual void	onEntry ()
virtual void	onExit ()
template<typename EventType >
void	postEvent (const EventType &ev)
template<typename EventType >
void	postEvent ()
ISmaccState *	getCurrentState ()
virtual void	dispose ()
virtual rclcpp::Node::SharedPtr	getNode ()
virtual rclcpp::Logger	getLogger ()
Protected Attributes inherited from cl_nav2z::CbNav2ZClientBehaviorBase
cl_nav2z::ClNav2Z *	moveBaseClient_
rclcpp_action::ResultCode	navigationResult_

Detailed Description

Definition at line 28 of file cb_undo_path_backwards.hpp.

Member Function Documentation

onEntry()

void cl_nav2z::CbUndoPathBackwards::onEntry ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 34 of file cb_undo_path_backwards.cpp.

{
  listener = std::make_shared<tf2_ros::Buffer>(this->getNode()->get_clock());
  auto * odomTracker = moveBaseClient_->getComponent<OdomTracker>();
 
  auto plannerSwitcher = moveBaseClient_->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 = moveBaseClient_->getComponent<GoalCheckerSwitcher>();
  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
  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();
    plannerSwitcher->setUndoPathBackwardPlanner();
    moveBaseClient_->sendGoal(goal);
  }
}

References smacc2::ISmaccClient::getComponent(), smacc2::ISmaccClientBehavior::getNode(), listener, cl_nav2z::CbNav2ZClientBehaviorBase::moveBaseClient_, service_node_3::s, smacc2::client_bases::SmaccActionClientBase< ActionType >::sendGoal(), and cl_nav2z::GoalCheckerSwitcher::setGoalCheckerId().

Here is the call graph for this function:

onExit()

void cl_nav2z::CbUndoPathBackwards::onExit ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 65 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() << " - Exiting: undo navigation successful, popping odom tracker path");
    auto * odomTracker = moveBaseClient_->getComponent<OdomTracker>();
    odomTracker->popPath();
  }
  else
  {
    // 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::CbNav2ZClientBehaviorBase::moveBaseClient_, cl_nav2z::CbNav2ZClientBehaviorBase::navigationResult_, and cl_nav2z::odom_tracker::OdomTracker::popPath().

Here is the call graph for this function:

Member Data Documentation

goalChecker_

std::optional<std::string> cl_nav2z::CbUndoPathBackwards::goalChecker_

Definition at line 31 of file cb_undo_path_backwards.hpp.

listener

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

private

Definition at line 38 of file cb_undo_path_backwards.hpp.

Referenced by 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