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

#include <cb_rotate_look_at.hpp>

Inheritance diagram for cl_nav2z::CbRotateLookAt:
Inheritance graph
Collaboration diagram for cl_nav2z::CbRotateLookAt:
Collaboration graph

Public Member Functions

 CbRotateLookAt ()
 
 CbRotateLookAt (const geometry_msgs::msg::PoseStamped &lookAtPose)
 
void onEntry () override
 
- Public Member Functions inherited from cl_nav2z::CbAbsoluteRotate
 CbAbsoluteRotate ()
 
 CbAbsoluteRotate (float absoluteGoalAngleDegree, float yawGoalTolerance=-1)
 
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)
 
virtual void onEntry ()
 
virtual void onExit ()
 
virtual void executeOnEntry ()
 
virtual void executeOnExit ()
 

Public Attributes

std::shared_ptr< tf2_ros::Buffer > listener
 
std::optional< geometry_msgs::msg::PoseStamped > lookAtPose_
 
- Public Attributes inherited from cl_nav2z::CbAbsoluteRotate
std::shared_ptr< tf2_ros::Buffer > listener
 
float absoluteGoalAngleDegree
 
std::optional< float > yawGoalTolerance
 
std::optional< float > maxVelTheta
 
std::optional< SpinningPlannerspinningPlanner
 

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 ()
 
ISmaccStategetCurrentState ()
 
virtual void dispose ()
 
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 30 of file cb_rotate_look_at.hpp.

Constructor & Destructor Documentation

◆ CbRotateLookAt() [1/2]

cl_nav2z::CbRotateLookAt::CbRotateLookAt ( )

Definition at line 32 of file cb_rotate_look_at.cpp.

32{}

◆ CbRotateLookAt() [2/2]

cl_nav2z::CbRotateLookAt::CbRotateLookAt ( const geometry_msgs::msg::PoseStamped &  lookAtPose)

Definition at line 34 of file cb_rotate_look_at.cpp.

35: lookAtPose_(lookAtPose)
36{
37}
std::optional< geometry_msgs::msg::PoseStamped > lookAtPose_

Member Function Documentation

◆ onEntry()

void cl_nav2z::CbRotateLookAt::onEntry ( )
overridevirtual

Reimplemented from cl_nav2z::CbAbsoluteRotate.

Definition at line 39 of file cb_rotate_look_at.cpp.

40{
41 cl_nav2z::Pose * pose;
42 this->requiresComponent(pose);
43
44 pose->waitTransformUpdate(rclcpp::Rate(20));
45 auto position = pose->toPoseMsg().position;
46
47 if (lookAtPose_)
48 {
49 auto targetPosition = lookAtPose_->pose.position;
50 double yaw_degrees =
51 atan2(targetPosition.y - position.y, targetPosition.x - position.x) * 180.0 / M_PI;
52 this->absoluteGoalAngleDegree = yaw_degrees;
53 }
54
56}
geometry_msgs::msg::Pose toPoseMsg()
Definition: cp_pose.hpp:57
void waitTransformUpdate(rclcpp::Rate r=rclcpp::Rate(20))
Definition: cp_pose.cpp:77
void requiresComponent(SmaccComponentType *&storage, bool throwExceptionIfNotExist=false)

References cl_nav2z::CbAbsoluteRotate::absoluteGoalAngleDegree, lookAtPose_, cl_nav2z::CbAbsoluteRotate::onEntry(), smacc2::ISmaccClientBehavior::requiresComponent(), cl_nav2z::Pose::toPoseMsg(), and cl_nav2z::Pose::waitTransformUpdate().

Here is the call graph for this function:

Member Data Documentation

◆ listener

std::shared_ptr<tf2_ros::Buffer> cl_nav2z::CbRotateLookAt::listener

Definition at line 33 of file cb_rotate_look_at.hpp.

◆ lookAtPose_

std::optional<geometry_msgs::msg::PoseStamped> cl_nav2z::CbRotateLookAt::lookAtPose_

Definition at line 35 of file cb_rotate_look_at.hpp.

Referenced by onEntry().


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