cl_nav2z::CbNavigateBackwards Class Reference

#include <cb_navigate_backwards.hpp>

Inheritance diagram for cl_nav2z::CbNavigateBackwards:

Collaboration diagram for cl_nav2z::CbNavigateBackwards:

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

Public Attributes
float	backwardDistance
std::optional< float >	backwardSpeed
std::optional< std::string >	goalChecker_
cl_nav2z::odom_tracker::OdomTracker *	odomTracker_

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

{
  if (backwardDistance < 0)
  {
    RCLCPP_ERROR(getLogger(), "[CbNavigateBackwards] distance must be greater or equal than 0");
    this->backwardDistance = 0;
  }
  this->backwardDistance = backwardDistance;
}

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.

{
  // straight motion distance
  double dist = backwardDistance;
 
  RCLCPP_INFO_STREAM(
    getLogger(), "[CbNavigateBackwards] Straight backwards motion distance: " << dist);
 
  auto p = nav2zClient_->getComponent<cl_nav2z::Pose>();
  auto referenceFrame = p->getReferenceFrame();
  auto currentPoseMsg = p->toPoseMsg();
  tf2::Transform currentPose;
  tf2::fromMsg(currentPoseMsg, currentPose);
 
  tf2::Transform backwardDeltaTransform;
  backwardDeltaTransform.setIdentity();
  backwardDeltaTransform.setOrigin(tf2::Vector3(-dist, 0, 0));
 
  tf2::Transform targetPose = currentPose * backwardDeltaTransform;
 
  ClNav2Z::Goal goal;
  goal.pose.header.frame_id = referenceFrame;
  //goal.pose.header.stamp = getNode()->now();
  tf2::toMsg(targetPose, goal.pose.pose);
  RCLCPP_INFO_STREAM(getLogger(), "[CbNavigateBackwards] TARGET POSE BACKWARDS: " << goal.pose);
 
  geometry_msgs::msg::PoseStamped currentStampedPoseMsg;
  currentStampedPoseMsg.header.frame_id = referenceFrame;
  currentStampedPoseMsg.header.stamp = getNode()->now();
  tf2::toMsg(currentPose, currentStampedPoseMsg.pose);
 
  odomTracker_ = nav2zClient_->getComponent<OdomTracker>();
  if (odomTracker_ != nullptr)
  {
    this->odomTracker_->clearPath();
    this->odomTracker_->setStartPoint(currentStampedPoseMsg);
    this->odomTracker_->setWorkingMode(WorkingMode::RECORD_PATH);
  }
 
  auto plannerSwitcher = nav2zClient_->getComponent<PlannerSwitcher>();
  plannerSwitcher->setBackwardPlanner();
 
  auto goalCheckerSwitcher = nav2zClient_->getComponent<GoalCheckerSwitcher>();
  goalCheckerSwitcher->setGoalCheckerId("backward_goal_checker");
 
  this->sendGoal(goal);
}

References backwardDistance, cl_nav2z::odom_tracker::OdomTracker::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::PlannerSwitcher::setBackwardPlanner(), cl_nav2z::GoalCheckerSwitcher::setGoalCheckerId(), cl_nav2z::odom_tracker::OdomTracker::setStartPoint(), and cl_nav2z::odom_tracker::OdomTracker::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.

{
  if (odomTracker_)
  {
    this->odomTracker_->setWorkingMode(WorkingMode::IDLE);
  }
}

References odomTracker_, and cl_nav2z::odom_tracker::OdomTracker::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::OdomTracker* 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:

• smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_navigate_backwards.hpp
• smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_backward.cpp