SMACC2
Loading...
Searching...
No Matches
Public Member Functions | Private Attributes | List of all members
cl_nav2z::CbUndoPathBackwards Class Reference

#include <cb_undo_path_backwards.hpp>

Inheritance diagram for cl_nav2z::CbUndoPathBackwards:
Inheritance graph
Collaboration diagram for cl_nav2z::CbUndoPathBackwards:
Collaboration graph

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 ()
 
ISmaccStateMachinegetStateMachine ()
 
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
 
OdomTrackerodomTracker
 
std::optional< CbUndoPathBackwardsOptionsoptions_
 

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 ()
 
ISmaccStategetCurrentState ()
 
virtual void dispose ()
 
virtual rclcpp::Node::SharedPtr getNode () const
 
virtual rclcpp::Logger getLogger () const
 
- Protected Attributes inherited from cl_nav2z::CbNav2ZClientBehaviorBase
cl_nav2z::ClNav2Znav2zClient_
 
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.

34{
35 options_ = options;
36}
std::optional< CbUndoPathBackwardsOptions > 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.

39{
40 listener = std::make_shared<tf2_ros::Buffer>(this->getNode()->get_clock());
41 odomTracker = nav2zClient_->getComponent<OdomTracker>();
42
44
45 auto plannerSwitcher = nav2zClient_->getComponent<PlannerSwitcher>();
46
47 nav_msgs::msg::Path forwardpath = odomTracker->getPath();
48 // RCLCPP_INFO_STREAM(getLogger(),"[UndoPathBackward] Current path backwards: " << forwardpath);
49
50 odomTracker->setWorkingMode(WorkingMode::CLEAR_PATH);
51
52 ClNav2Z::Goal goal;
53
54 auto goalCheckerSwitcher = nav2zClient_->getComponent<GoalCheckerSwitcher>();
55
56 if (options_ && options_->goalCheckerId_)
57 {
58 goalCheckerSwitcher->setGoalCheckerId(*options_->goalCheckerId_);
59 }
60 else
61 {
62 goalCheckerSwitcher->setGoalCheckerId("undo_path_backwards_goal_checker");
63 }
64
65 // WARNING: There might be some race condition with the remote undo global planner were the global path was not
66 // received yet
67 // TODO: waiting notification from global planner that it is loaded
68 rclcpp::sleep_for(1s);
69
70 // this line is used to flush/reset backward planner in the case it were already there
71 // plannerSwitcher->setDefaultPlanners();
72 if (forwardpath.poses.size() > 0)
73 {
74 goal.pose = forwardpath.poses.front();
75 //goal.pose.header.stamp = getNode()->now();
76 goal.pose.header.stamp = rclcpp::Time(0);
77
78 if (options_ && options_->undoControllerName_)
79 {
80 plannerSwitcher->setUndoPathBackwardPlanner(false);
81 RCLCPP_INFO_STREAM(
82 getLogger(),
83 "[" << getName() << "] Undoing path with controller: " << *options_->undoControllerName_);
84 plannerSwitcher->setDesiredController(*options_->undoControllerName_);
85 plannerSwitcher->commitPublish();
86 }
87 else
88 {
89 plannerSwitcher->setUndoPathBackwardPlanner();
90 }
91
92 this->sendGoal(goal);
93 }
94}
std::shared_ptr< tf2_ros::Buffer > listener
void logStateString(bool debug=true)
void setWorkingMode(WorkingMode workingMode)
virtual rclcpp::Logger getLogger() const
virtual rclcpp::Node::SharedPtr getNode() const
TComponent * getComponent()

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.

97{
98 RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Exiting: undo navigation ");
99
100 if (this->navigationResult_ == rclcpp_action::ResultCode::SUCCEEDED)
101 {
102 RCLCPP_INFO_STREAM(
103 getLogger(), getName() << " - [CbUndoPathBackwards] Exiting: undo navigation successful, "
104 "popping odom tracker path");
105 odomTracker = nav2zClient_->getComponent<OdomTracker>();
107
109 }
110 else
111 {
112 RCLCPP_INFO_STREAM(
113 getLogger(), getName() << " - [CbUndoPathBackwards] Exiting: undo navigation abort, avoiding "
114 "popping current path");
115
116 odomTracker = nav2zClient_->getComponent<OdomTracker>();
118 // navigation interrupted or aborted. The path may be not totally undone.
119 // We keep the odom tracker in its current state, probably in the middle of the undoing process.
120 // Could you try to repeat the behavior?
121 }
122}
void popPath(int pathCount=1, bool keepPreviousPath=false)

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: