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

#include <cb_navigate_backwards.hpp>

Inheritance diagram for cl_nav2z::CbNavigateBackwards:
Inheritance graph
Collaboration diagram for cl_nav2z::CbNavigateBackwards:
Collaboration graph

Public Member Functions

 CbNavigateBackwards (float backwardDistanceMeters)
 
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)
 

Public Attributes

float backwardDistance
 
std::optional< float > backwardSpeed
 
std::optional< std::string > goalChecker_
 
cl_nav2z::odom_tracker::CpOdomTrackerodomTracker_
 

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 ()
 onEntry is executed in a new thread. However the current state cannot be left until the onEntry thread finishes. This flag can be checked from the onEntry thread to force finishing the thread.
 
- 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 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 32 of file cb_navigate_backwards.hpp.

Constructor & Destructor Documentation

◆ CbNavigateBackwards()

cl_nav2z::CbNavigateBackwards::CbNavigateBackwards ( float  backwardDistanceMeters)

Definition at line 34 of file cb_navigate_backward.cpp.

35{
36 if (backwardDistance < 0)
37 {
38 RCLCPP_ERROR(getLogger(), "[CbNavigateBackwards] distance must be greater or equal than 0");
39 this->backwardDistance = 0;
40 }
42}
virtual rclcpp::Logger getLogger() const

References backwardDistance, and smacc2::ISmaccClientBehavior::getLogger().

Here is the call graph for this function:

Member Function Documentation

◆ onEntry()

void cl_nav2z::CbNavigateBackwards::onEntry ( )
overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 44 of file cb_navigate_backward.cpp.

45{
46 // straight motion distance
47 double dist = backwardDistance;
48
49 RCLCPP_INFO_STREAM(
50 getLogger(), "[CbNavigateBackwards] Straight backwards motion distance: " << dist);
51
53 auto referenceFrame = p->getReferenceFrame();
54 auto currentPoseMsg = p->toPoseMsg();
55 tf2::Transform currentPose;
56 tf2::fromMsg(currentPoseMsg, currentPose);
57
58 tf2::Transform backwardDeltaTransform;
59 backwardDeltaTransform.setIdentity();
60 backwardDeltaTransform.setOrigin(tf2::Vector3(-dist, 0, 0));
61
62 tf2::Transform targetPose = currentPose * backwardDeltaTransform;
63
64 ClNav2Z::Goal goal;
65 goal.pose.header.frame_id = referenceFrame;
66 //goal.pose.header.stamp = getNode()->now();
67 tf2::toMsg(targetPose, goal.pose.pose);
68 RCLCPP_INFO_STREAM(getLogger(), "[CbNavigateBackwards] TARGET POSE BACKWARDS: " << goal.pose);
69
70 geometry_msgs::msg::PoseStamped currentStampedPoseMsg;
71 currentStampedPoseMsg.header.frame_id = referenceFrame;
72 currentStampedPoseMsg.header.stamp = getNode()->now();
73 tf2::toMsg(currentPose, currentStampedPoseMsg.pose);
74
75 odomTracker_ = nav2zClient_->getComponent<CpOdomTracker>();
76 if (odomTracker_ != nullptr)
77 {
78 this->odomTracker_->clearPath();
79 this->odomTracker_->setStartPoint(currentStampedPoseMsg);
80 this->odomTracker_->setWorkingMode(WorkingMode::RECORD_PATH);
81 }
82
83 auto plannerSwitcher = nav2zClient_->getComponent<CpPlannerSwitcher>();
84 plannerSwitcher->setBackwardPlanner();
85
86 auto goalCheckerSwitcher = nav2zClient_->getComponent<CpGoalCheckerSwitcher>();
87 goalCheckerSwitcher->setGoalCheckerId("backward_goal_checker");
88
89 this->sendGoal(goal);
90}
cl_nav2z::odom_tracker::CpOdomTracker * odomTracker_
const std::string & getReferenceFrame() const
Definition cp_pose.hpp:76
void setWorkingMode(WorkingMode workingMode)
void setStartPoint(const geometry_msgs::msg::PoseStamped &pose)
virtual rclcpp::Node::SharedPtr getNode() const

References backwardDistance, cl_nav2z::odom_tracker::CpOdomTracker::clearPath(), smacc2::ISmaccClient::getComponent(), smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getNode(), cl_nav2z::Pose::getReferenceFrame(), cl_nav2z::CbNav2ZClientBehaviorBase::nav2zClient_, odomTracker_, cl_nav2z::CbNav2ZClientBehaviorBase::sendGoal(), cl_nav2z::CpPlannerSwitcher::setBackwardPlanner(), cl_nav2z::CpGoalCheckerSwitcher::setGoalCheckerId(), cl_nav2z::odom_tracker::CpOdomTracker::setStartPoint(), and cl_nav2z::odom_tracker::CpOdomTracker::setWorkingMode().

Here is the call graph for this function:

◆ onExit()

void cl_nav2z::CbNavigateBackwards::onExit ( )
overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 92 of file cb_navigate_backward.cpp.

93{
94 if (odomTracker_)
95 {
96 this->odomTracker_->setWorkingMode(WorkingMode::IDLE);
97 }
98}

References odomTracker_, and cl_nav2z::odom_tracker::CpOdomTracker::setWorkingMode().

Here is the call graph for this function:

Member Data Documentation

◆ backwardDistance

float cl_nav2z::CbNavigateBackwards::backwardDistance

Definition at line 35 of file cb_navigate_backwards.hpp.

Referenced by CbNavigateBackwards(), and onEntry().

◆ backwardSpeed

std::optional<float> cl_nav2z::CbNavigateBackwards::backwardSpeed

Definition at line 38 of file cb_navigate_backwards.hpp.

◆ goalChecker_

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

Definition at line 40 of file cb_navigate_backwards.hpp.

◆ odomTracker_

cl_nav2z::odom_tracker::CpOdomTracker* cl_nav2z::CbNavigateBackwards::odomTracker_

Definition at line 42 of file cb_navigate_backwards.hpp.

Referenced by onEntry(), and onExit().


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