SMACC2
Loading...
Searching...
No Matches
cl_nav2z::CbNavigateGlobalPosition Class Reference

#include <cb_navigate_global_position.hpp>

Inheritance diagram for cl_nav2z::CbNavigateGlobalPosition:
Inheritance graph
Collaboration diagram for cl_nav2z::CbNavigateGlobalPosition:
Collaboration graph

Public Member Functions

 CbNavigateGlobalPosition ()
 
 CbNavigateGlobalPosition (float x, float y, float yaw, std::optional< CbNavigateGlobalPositionOptions > options=std::nullopt)
 
void setGoal (const geometry_msgs::msg::Pose &pose)
 
virtual void onEntry () override
 
void onExit () override
 
void execute ()
 
 CbNavigateGlobalPosition ()
 
 CbNavigateGlobalPosition (float x, float y, float yaw, std::optional< CbNavigateGlobalPositionOptions > options=std::nullopt)
 
void setGoal (const geometry_msgs::msg::Pose &pose)
 
virtual void onEntry () override
 
void onExit () override
 
void execute ()
 
- Public Member Functions inherited from cl_nav2z::CbNav2ZClientBehaviorBase
virtual ~CbNav2ZClientBehaviorBase ()
 
template<typename TOrthogonal , typename TSourceObject >
void onStateOrthogonalAllocation ()
 
template<typename TOrthogonal , typename TSourceObject >
void onOrthogonalAllocation ()
 
virtual ~CbNav2ZClientBehaviorBase ()
 
template<typename TOrthogonal , typename TSourceObject >
void onOrthogonalAllocation ()
 
template<typename TOrthogonal , typename TSourceObject >
void onStateOrthogonalAllocation ()
 
- Public Member Functions inherited from smacc2::SmaccAsyncClientBehavior
template<typename TOrthogonal , typename TSourceObject >
void onOrthogonalAllocation ()
 
template<typename TOrthogonal , typename TSourceObject >
void onStateOrthogonalAllocation ()
 
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)
 
template<typename SmaccComponentType >
void requiresComponent (SmaccComponentType *&storage, ComponentRequirement requirementType=ComponentRequirement::SOFT)
 

Public Attributes

float goalYaw
 
CbNavigateGlobalPositionOptions options
 
geometry_msgs::msg::Point goalPosition
 

Private Member Functions

void readStartPoseFromParameterServer (nav2_msgs::action::NavigateToPose::Goal &goal)
 
void readStartPoseFromParameterServer (ClNav2Z::Goal &goal)
 

Additional Inherited Members

- Protected Member Functions inherited from cl_nav2z::CbNav2ZClientBehaviorBase
void sendGoal (nav2_msgs::action::NavigateToPose::Goal &goal)
 
void cancelGoal ()
 
template<typename T >
boost::signals2::connection onNavigationSucceeded (void(T::*callback)(const components::CpNav2ActionInterface::WrappedResult &), T *object)
 
template<typename T >
boost::signals2::connection onNavigationAborted (void(T::*callback)(const components::CpNav2ActionInterface::WrappedResult &), T *object)
 
template<typename T >
boost::signals2::connection onNavigationCancelled (void(T::*callback)(const components::CpNav2ActionInterface::WrappedResult &), T *object)
 
virtual void onNavigationResult (const components::CpNav2ActionInterface::WrappedResult &)
 
virtual void onNavigationActionSuccess (const components::CpNav2ActionInterface::WrappedResult &)
 
virtual void onNavigationActionAbort (const components::CpNav2ActionInterface::WrappedResult &)
 
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
components::CpNav2ActionInterfacenav2ActionInterface_ = nullptr
 
smacc2::client_core_components::CpActionClient< nav2_msgs::action::NavigateToPose > * actionClient_ = nullptr
 
rclcpp_action::ResultCode navigationResult_
 
cl_nav2z::ClNav2Znav2zClient_
 
cl_nav2z::ClNav2Z::SmaccNavigateResultSignal::SharedPtr navigationCallback_
 
std::shared_future< std::shared_ptr< rclcpp_action::ClientGoalHandle< nav2_msgs::action::NavigateToPose > > > goalHandleFuture_
 

Detailed Description

Definition at line 40 of file cb_navigate_global_position.hpp.

Constructor & Destructor Documentation

◆ CbNavigateGlobalPosition() [1/4]

cl_nav2z::CbNavigateGlobalPosition::CbNavigateGlobalPosition ( )

Definition at line 33 of file cb_navigate_global_position.cpp.

33{}

◆ CbNavigateGlobalPosition() [2/4]

cl_nav2z::CbNavigateGlobalPosition::CbNavigateGlobalPosition ( float x,
float y,
float yaw,
std::optional< CbNavigateGlobalPositionOptions > options = std::nullopt )

Definition at line 35 of file cb_navigate_global_position.cpp.

37{
38 auto p = geometry_msgs::msg::Point();
39 p.x = x;
40 p.y = y;
41 goalPosition = p;
42 goalYaw = yaw;
43
44 if (options) this->options = *options;
45}

References goalPosition, goalYaw, and options.

◆ CbNavigateGlobalPosition() [3/4]

cl_nav2z::CbNavigateGlobalPosition::CbNavigateGlobalPosition ( )

◆ CbNavigateGlobalPosition() [4/4]

cl_nav2z::CbNavigateGlobalPosition::CbNavigateGlobalPosition ( float x,
float y,
float yaw,
std::optional< CbNavigateGlobalPositionOptions > options = std::nullopt )

Member Function Documentation

◆ execute() [1/2]

void cl_nav2z::CbNavigateGlobalPosition::execute ( )

Definition at line 90 of file cb_navigate_global_position.cpp.

91{
93 this->requiresComponent(p, ComponentRequirement::HARD);
94
95 auto referenceFrame = p->getReferenceFrame();
96 // auto currentPoseMsg = p->toPoseMsg();
97
98 RCLCPP_INFO(getLogger(), "Sending Goal to MoveBase");
99 nav2_msgs::action::NavigateToPose::Goal goal;
100 goal.pose.header.frame_id = referenceFrame;
101 //goal.pose.header.stamp = getNode()->now();
102
103 goal.pose.pose.position = goalPosition;
104 tf2::Quaternion q;
105 q.setRPY(0, 0, goalYaw);
106 goal.pose.pose.orientation = tf2::toMsg(q);
107
108 this->sendGoal(goal);
109}
void sendGoal(nav2_msgs::action::NavigateToPose::Goal &goal)
const std::string & getReferenceFrame() const
Definition cp_pose.hpp:79
void requiresComponent(SmaccComponentType *&storage, bool throwExceptionIfNotExist)
virtual rclcpp::Logger getLogger() const

References smacc2::ISmaccClientBehavior::getLogger(), cl_nav2z::CpPose::getReferenceFrame(), goalPosition, goalYaw, smacc2::ISmaccClientBehavior::requiresComponent(), and cl_nav2z::CbNav2ZClientBehaviorBase::sendGoal().

Referenced by onEntry().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ execute() [2/2]

void cl_nav2z::CbNavigateGlobalPosition::execute ( )

◆ onEntry() [1/2]

void cl_nav2z::CbNavigateGlobalPosition::onEntry ( )
overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Reimplemented in cl_nav2z::CbStopNavigation, and cl_nav2z::CbStopNavigation.

Definition at line 53 of file cb_navigate_global_position.cpp.

54{
55 RCLCPP_INFO(getLogger(), "Entering Navigate Global position");
56 RCLCPP_INFO(getLogger(), "Component requirements completed");
57
58 cl_nav2z::CpPose * cpPose;
59 this->requiresComponent(cpPose, ComponentRequirement::HARD);
60 auto pose = cpPose->toPoseMsg();
61
62 CpOdomTracker * odomTracker;
63 this->requiresComponent(odomTracker, ComponentRequirement::HARD);
64
65 CpPlannerSwitcher * plannerSwitcher;
66 this->requiresComponent(plannerSwitcher, ComponentRequirement::HARD);
67
68 plannerSwitcher->setDefaultPlanners(false);
69
71 {
72 plannerSwitcher->setDesiredController(*(options.controllerName_));
73 }
74
75 plannerSwitcher->commitPublish();
76
77 CpGoalCheckerSwitcher * goalCheckerSwitcher;
78 this->requiresComponent(goalCheckerSwitcher, ComponentRequirement::HARD);
79 goalCheckerSwitcher->setGoalCheckerId("goal_checker");
80
81 auto pathname = this->getCurrentState()->getName() + " - " + getName();
82 odomTracker->pushPath(pathname);
83 odomTracker->setStartPoint(pose);
84 odomTracker->setWorkingMode(WorkingMode::RECORD_PATH);
85
86 execute();
87}
geometry_msgs::msg::Pose toPoseMsg()
Definition cp_pose.hpp:57
virtual std::string getName()=0

References cl_nav2z::CpPlannerSwitcher::commitPublish(), cl_nav2z::CbNavigateGlobalPositionOptions::controllerName_, execute(), smacc2::ISmaccClientBehavior::getCurrentState(), smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), smacc2::ISmaccState::getName(), options, cl_nav2z::odom_tracker::CpOdomTracker::pushPath(), smacc2::ISmaccClientBehavior::requiresComponent(), cl_nav2z::CpPlannerSwitcher::setDefaultPlanners(), cl_nav2z::CpPlannerSwitcher::setDesiredController(), cl_nav2z::CpGoalCheckerSwitcher::setGoalCheckerId(), cl_nav2z::odom_tracker::CpOdomTracker::setStartPoint(), cl_nav2z::odom_tracker::CpOdomTracker::setWorkingMode(), and cl_nav2z::CpPose::toPoseMsg().

Referenced by cl_nav2z::CbStopNavigation::onEntry().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ onEntry() [2/2]

virtual void cl_nav2z::CbNavigateGlobalPosition::onEntry ( )
overridevirtual

◆ onExit() [1/2]

void cl_nav2z::CbNavigateGlobalPosition::onExit ( )
overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Reimplemented in cl_nav2z::CbStopNavigation, and cl_nav2z::CbStopNavigation.

Definition at line 113 of file cb_navigate_global_position.cpp.

114{
115 RCLCPP_INFO(getLogger(), "Exiting move goal Action Client");
116}

References smacc2::ISmaccClientBehavior::getLogger().

Here is the call graph for this function:

◆ onExit() [2/2]

void cl_nav2z::CbNavigateGlobalPosition::onExit ( )
overridevirtual

◆ readStartPoseFromParameterServer() [1/2]

void cl_nav2z::CbNavigateGlobalPosition::readStartPoseFromParameterServer ( ClNav2Z::Goal & goal)
private

◆ readStartPoseFromParameterServer() [2/2]

void cl_nav2z::CbNavigateGlobalPosition::readStartPoseFromParameterServer ( nav2_msgs::action::NavigateToPose::Goal & goal)
private

◆ setGoal() [1/2]

void cl_nav2z::CbNavigateGlobalPosition::setGoal ( const geometry_msgs::msg::Pose & pose)

Definition at line 47 of file cb_navigate_global_position.cpp.

48{
49 goalPosition = pose.position;
50 goalYaw = tf2::getYaw(pose.orientation);
51}

References goalPosition, and goalYaw.

Referenced by cl_nav2z::CbStopNavigation::onEntry().

Here is the caller graph for this function:

◆ setGoal() [2/2]

void cl_nav2z::CbNavigateGlobalPosition::setGoal ( const geometry_msgs::msg::Pose & pose)

Member Data Documentation

◆ goalPosition

geometry_msgs::msg::Point cl_nav2z::CbNavigateGlobalPosition::goalPosition

Definition at line 47 of file cb_navigate_global_position.hpp.

Referenced by CbNavigateGlobalPosition(), execute(), and setGoal().

◆ goalYaw

float cl_nav2z::CbNavigateGlobalPosition::goalYaw

Definition at line 44 of file cb_navigate_global_position.hpp.

Referenced by CbNavigateGlobalPosition(), execute(), and setGoal().

◆ options

CbNavigateGlobalPositionOptions cl_nav2z::CbNavigateGlobalPosition::options

Definition at line 46 of file cb_navigate_global_position.hpp.

Referenced by CbNavigateGlobalPosition(), and onEntry().


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