#include <cb_navigate_forward.hpp>

Inheritance diagram for cl_nav2z::CbNavigateForward:

Collaboration diagram for cl_nav2z::CbNavigateForward:

Public Member Functions
	CbNavigateForward ()

	CbNavigateForward (float forwardDistance)

	CbNavigateForward (geometry_msgs::msg::PoseStamped goalPosition)

virtual	~CbNavigateForward ()

void	onEntry () override

void	onExit () override

void	setForwardDistance (float distance_meters)

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
CbNavigateForwardOptions	options

Protected Attributes
odom_tracker::OdomTracker *	odomTracker_

std::optional< geometry_msgs::msg::PoseStamped >	goalPose_

std::optional< float >	forwardDistance_

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_

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

Detailed Description

Definition at line 48 of file cb_navigate_forward.hpp.

Constructor & Destructor Documentation

◆ CbNavigateForward() [1/3]

cl_nav2z::CbNavigateForward::CbNavigateForward ( )

Definition at line 36 of file cb_navigate_forward.cpp.

36{}

◆ CbNavigateForward() [2/3]

cl_nav2z::CbNavigateForward::CbNavigateForward ( float forwardDistance )

Definition at line 35 of file cb_navigate_forward.cpp.

35: forwardDistance_(distance_meters) {}

cl_nav2z::CbNavigateForward::forwardDistance_

std::optional< float > forwardDistance_

Definition: cb_navigate_forward.hpp:77

◆ CbNavigateForward() [3/3]

cl_nav2z::CbNavigateForward::CbNavigateForward ( geometry_msgs::msg::PoseStamped goalPosition )

Definition at line 38 of file cb_navigate_forward.cpp.

38: goalPose_(goal) {}

cl_nav2z::CbNavigateForward::goalPose_

std::optional< geometry_msgs::msg::PoseStamped > goalPose_

Definition: cb_navigate_forward.hpp:75

◆ ~CbNavigateForward()

cl_nav2z::CbNavigateForward::~CbNavigateForward ( )

virtual

Definition at line 40 of file cb_navigate_forward.cpp.

40{}

Member Function Documentation

◆ onEntry()

void cl_nav2z::CbNavigateForward::onEntry ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 57 of file cb_navigate_forward.cpp.

{
  if (forwardDistance_)
  {
    setForwardDistance(*forwardDistance_);
 
    RCLCPP_INFO_STREAM(
      getLogger(), "[" << getName() << "] Straight motion distance: " << *forwardDistance_);
  }
 
  // get current pose
  auto p = nav2zClient_->getComponent<Pose>();
  auto referenceFrame = p->getReferenceFrame();
  auto currentPoseMsg = p->toPoseMsg();
  tf2::Transform currentPose;
  tf2::fromMsg(currentPoseMsg, currentPose);
 
  RCLCPP_INFO_STREAM(
    getLogger(), "[" << getName() << "]"
                     << "current pose: " << currentPoseMsg);
 
  // force global orientation if it is requested
  if (options.forwardSpeed)
  {
    currentPoseMsg.orientation = *(options.forceInitialOrientation);
    RCLCPP_WARN_STREAM(
      getLogger(),
      "[" << getName() << "]"
          << "Forcing initial straight motion orientation: " << currentPoseMsg.orientation);
  }
 
  tf2::Transform targetPose;
  if (goalPose_)
  {
    tf2::fromMsg(goalPose_->pose, targetPose);
  }
  else if (forwardDistance_)
  {
    // compute forward goal pose
    tf2::Transform forwardDeltaTransform;
    forwardDeltaTransform.setIdentity();
    forwardDeltaTransform.setOrigin(tf2::Vector3(*forwardDistance_, 0, 0));
 
    targetPose = currentPose * forwardDeltaTransform;
  }
  else
  {
    RCLCPP_WARN_STREAM(
      getLogger(),
      "[" << getName() << "]"
          << "No goal Pose or Distance is specified. Aborting. No action request is sent."
          << currentPoseMsg.orientation);
 
    return;
  }
 
  // action goal
  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(), "[" << getName() << "]"
                     << " TARGET POSE FORWARD: " << goal.pose.pose);
 
  // current pose
  geometry_msgs::msg::PoseStamped currentStampedPoseMsg;
  currentStampedPoseMsg.header.frame_id = referenceFrame;
  currentStampedPoseMsg.header.stamp =
    getNode()->now();  // probably it is better avoid setting that goal timestamp
 
  tf2::toMsg(currentPose, currentStampedPoseMsg.pose);
 
  odomTracker_ = nav2zClient_->getComponent<OdomTracker>();
  if (odomTracker_ != nullptr)
  {
    auto pathname = this->getCurrentState()->getName() + " - " + getName();
    odomTracker_->pushPath(pathname);
    odomTracker_->setStartPoint(currentStampedPoseMsg);
    odomTracker_->setCurrentMotionGoal(goal.pose);
    odomTracker_->setWorkingMode(WorkingMode::RECORD_PATH);
  }
 
  auto plannerSwitcher = nav2zClient_->getComponent<PlannerSwitcher>();
  plannerSwitcher->setForwardPlanner();
 
  auto goalCheckerSwitcher = nav2zClient_->getComponent<GoalCheckerSwitcher>();
  goalCheckerSwitcher->setGoalCheckerId("forward_goal_checker");
 
  this->sendGoal(goal);
}

References cl_nav2z::CbNavigateForwardOptions::forceInitialOrientation, forwardDistance_, cl_nav2z::CbNavigateForwardOptions::forwardSpeed, smacc2::ISmaccClient::getComponent(), smacc2::ISmaccClientBehavior::getCurrentState(), smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), smacc2::ISmaccState::getName(), smacc2::ISmaccClientBehavior::getNode(), cl_nav2z::Pose::getReferenceFrame(), goalPose_, cl_nav2z::CbNav2ZClientBehaviorBase::nav2zClient_, odomTracker_, options, cl_nav2z::odom_tracker::OdomTracker::pushPath(), cl_nav2z::CbNav2ZClientBehaviorBase::sendGoal(), cl_nav2z::odom_tracker::OdomTracker::setCurrentMotionGoal(), setForwardDistance(), cl_nav2z::PlannerSwitcher::setForwardPlanner(), 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::CbNavigateForward::onExit ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 149 of file cb_navigate_forward.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:

◆ setForwardDistance()

void cl_nav2z::CbNavigateForward::setForwardDistance ( float distance_meters )

Definition at line 42 of file cb_navigate_forward.cpp.

{
  if (distance_meters < 0)
  {
    RCLCPP_INFO_STREAM(
      getLogger(), "[" << getName() << "] negative forward distance: " << distance_meters
                       << ". Resetting to 0.");
    distance_meters = 0;
  }
  forwardDistance_ = distance_meters;
 
  RCLCPP_INFO_STREAM(
    getLogger(), "[" << getName() << "] setting fw motion distance: " << *forwardDistance_);
}

References forwardDistance_, smacc2::ISmaccClientBehavior::getLogger(), and smacc2::ISmaccClientBehavior::getName().

Referenced by onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ forwardDistance_

std::optional<float> cl_nav2z::CbNavigateForward::forwardDistance_

protected

Definition at line 77 of file cb_navigate_forward.hpp.

Referenced by onEntry(), and setForwardDistance().

◆ goalPose_

std::optional<geometry_msgs::msg::PoseStamped> cl_nav2z::CbNavigateForward::goalPose_

protected

Definition at line 75 of file cb_navigate_forward.hpp.

Referenced by onEntry().

◆ odomTracker_

odom_tracker::OdomTracker* cl_nav2z::CbNavigateForward::odomTracker_

protected

Definition at line 73 of file cb_navigate_forward.hpp.

Referenced by onEntry(), and onExit().

◆ options

CbNavigateForwardOptions cl_nav2z::CbNavigateForward::options

Definition at line 51 of file cb_navigate_forward.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_navigate_forward.hpp
smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_forward.cpp

Public Member Functions

Public Attributes

Protected Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ CbNavigateForward() [1/3]

◆ CbNavigateForward() [2/3]

◆ CbNavigateForward() [3/3]

◆ ~CbNavigateForward()

Member Function Documentation

◆ onEntry()

◆ onExit()

◆ setForwardDistance()

Member Data Documentation

◆ forwardDistance_

◆ goalPose_

◆ odomTracker_

◆ options