SMACC2
Public Member Functions | Public Attributes | Protected Attributes | List of all members
cl_nav2z::CbNavigateForward Class Reference

#include <cb_navigate_forward.hpp>

Inheritance diagram for cl_nav2z::CbNavigateForward:
Inheritance graph
Collaboration diagram for cl_nav2z::CbNavigateForward:
Collaboration graph

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

Public Attributes

std::optional< float > forwardSpeed
 
std::optional< geometry_msgs::msg::Quaternion > forceInitialOrientation
 
std::optional< std::string > goalChecker_
 

Protected Attributes

odom_tracker::OdomTrackerodomTracker_
 
std::optional< geometry_msgs::msg::PoseStamped > goalPose_
 
std::optional< float > forwardDistance_
 
- Protected Attributes inherited from cl_nav2z::CbNav2ZClientBehaviorBase
cl_nav2z::ClNav2ZmoveBaseClient_
 
rclcpp_action::ResultCode navigationResult_
 

Additional Inherited Members

- 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 ()
 
virtual void onEntry ()
 
virtual void onExit ()
 
template<typename EventType >
void postEvent (const EventType &ev)
 
template<typename EventType >
void postEvent ()
 
ISmaccStategetCurrentState ()
 
virtual void dispose ()
 
virtual rclcpp::Node::SharedPtr getNode ()
 
virtual rclcpp::Logger getLogger ()
 

Detailed Description

Definition at line 34 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) {}
std::optional< float > forwardDistance_

◆ 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) {}
std::optional< geometry_msgs::msg::PoseStamped > goalPose_

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

58{
60 {
62
63 RCLCPP_INFO_STREAM(
64 getLogger(), "[" << getName() << "] Straight motion distance: " << *forwardDistance_);
65 }
66
67 // get current pose
68 auto p = moveBaseClient_->getComponent<Pose>();
69 auto referenceFrame = p->getReferenceFrame();
70 auto currentPoseMsg = p->toPoseMsg();
71 tf2::Transform currentPose;
72 tf2::fromMsg(currentPoseMsg, currentPose);
73
74 RCLCPP_INFO_STREAM(
75 getLogger(), "[" << getName() << "]"
76 << "current pose: " << currentPoseMsg);
77
78 // force global orientation if it is requested
80 {
81 currentPoseMsg.orientation = *forceInitialOrientation;
82 RCLCPP_WARN_STREAM(
83 getLogger(),
84 "[" << getName() << "]"
85 << "Forcing initial straight motion orientation: " << currentPoseMsg.orientation);
86 }
87
88 tf2::Transform targetPose;
89 if (goalPose_)
90 {
91 tf2::fromMsg(goalPose_->pose, targetPose);
92 }
93 else if (forwardDistance_)
94 {
95 // compute forward goal pose
96 tf2::Transform forwardDeltaTransform;
97 forwardDeltaTransform.setIdentity();
98 forwardDeltaTransform.setOrigin(tf2::Vector3(*forwardDistance_, 0, 0));
99
100 targetPose = currentPose * forwardDeltaTransform;
101 }
102 else
103 {
104 RCLCPP_WARN_STREAM(
105 getLogger(),
106 "[" << getName() << "]"
107 << "No goal Pose or Distance is specified. Aborting. No action request is sent."
108 << currentPoseMsg.orientation);
109
110 return;
111 }
112
113 // action goal
114 ClNav2Z::Goal goal;
115 goal.pose.header.frame_id = referenceFrame;
116 goal.pose.header.stamp = getNode()->now();
117 tf2::toMsg(targetPose, goal.pose.pose);
118 RCLCPP_INFO_STREAM(
119 getLogger(), "[" << getName() << "]"
120 << " TARGET POSE FORWARD: " << goal.pose.pose);
121
122 // current pose
123 geometry_msgs::msg::PoseStamped currentStampedPoseMsg;
124 currentStampedPoseMsg.header.frame_id = referenceFrame;
125 currentStampedPoseMsg.header.stamp = getNode()->now();
126 tf2::toMsg(currentPose, currentStampedPoseMsg.pose);
127
128 odomTracker_ = moveBaseClient_->getComponent<OdomTracker>();
129 if (odomTracker_ != nullptr)
130 {
131 auto pathname = this->getCurrentState()->getName() + " - " + getName();
132 odomTracker_->pushPath(pathname);
133 odomTracker_->setStartPoint(currentStampedPoseMsg);
135 odomTracker_->setWorkingMode(WorkingMode::RECORD_PATH);
136 }
137
138 auto plannerSwitcher = moveBaseClient_->getComponent<PlannerSwitcher>();
139 plannerSwitcher->setForwardPlanner();
140
141 auto goalCheckerSwitcher = moveBaseClient_->getComponent<GoalCheckerSwitcher>();
142 goalCheckerSwitcher->setGoalCheckerId("forward_goal_checker");
143
145}
void setForwardDistance(float distance_meters)
odom_tracker::OdomTracker * odomTracker_
std::optional< geometry_msgs::msg::Quaternion > forceInitialOrientation
void setCurrentMotionGoal(const geometry_msgs::msg::PoseStamped &pose)
void setWorkingMode(WorkingMode workingMode)
void setStartPoint(const geometry_msgs::msg::PoseStamped &pose)
virtual rclcpp::Node::SharedPtr getNode()
TComponent * getComponent()
virtual std::string getName()=0
std::shared_future< typename GoalHandle::SharedPtr > sendGoal(Goal &goal)

References forceInitialOrientation, forwardDistance_, 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::moveBaseClient_, odomTracker_, cl_nav2z::odom_tracker::OdomTracker::pushPath(), smacc2::client_bases::SmaccActionClientBase< ActionType >::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 147 of file cb_navigate_forward.cpp.

148{
149 if (odomTracker_)
150 {
151 this->odomTracker_->setWorkingMode(WorkingMode::IDLE);
152 }
153}

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.

43{
44 if (distance_meters < 0)
45 {
46 RCLCPP_INFO_STREAM(
47 getLogger(), "[" << getName() << "] negative forward distance: " << distance_meters
48 << ". Resetting to 0.");
49 distance_meters = 0;
50 }
51 forwardDistance_ = distance_meters;
52
53 RCLCPP_INFO_STREAM(
54 getLogger(), "[" << getName() << "] setting fw motion distance: " << *forwardDistance_);
55}

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

Referenced by onEntry(), and sm_dance_bot_strikes_back::radial_motion_states::StiRadialEndPoint::runtimeConfigure().

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

Member Data Documentation

◆ forceInitialOrientation

std::optional<geometry_msgs::msg::Quaternion> cl_nav2z::CbNavigateForward::forceInitialOrientation

Definition at line 43 of file cb_navigate_forward.hpp.

Referenced by onEntry().

◆ forwardDistance_

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

Definition at line 72 of file cb_navigate_forward.hpp.

Referenced by onEntry(), and setForwardDistance().

◆ forwardSpeed

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

Definition at line 38 of file cb_navigate_forward.hpp.

◆ goalChecker_

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

Definition at line 46 of file cb_navigate_forward.hpp.

◆ goalPose_

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

Definition at line 70 of file cb_navigate_forward.hpp.

Referenced by onEntry().

◆ odomTracker_

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

Definition at line 68 of file cb_navigate_forward.hpp.

Referenced by onEntry(), and onExit().


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