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

#include <cb_rotate.hpp>

Inheritance diagram for cl_nav2z::CbRotate:
Inheritance graph
Collaboration diagram for cl_nav2z::CbRotate:
Collaboration graph

Public Member Functions

 CbRotate (float rotate_degree)
 
 CbRotate (float rotate_degree, cl_nav2z::SpinningPlanner spinning_planner)
 
void onEntry () 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

float rotateDegree
 
std::optional< std::string > goalChecker_
 
std::optional< cl_nav2z::SpinningPlannerspinningPlanner
 

Private Attributes

std::shared_ptr< tf2_ros::Buffer > listener
 

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 28 of file cb_rotate.hpp.

Constructor & Destructor Documentation

◆ CbRotate() [1/2]

cl_nav2z::CbRotate::CbRotate ( float  rotate_degree)

Definition at line 29 of file cb_rotate.cpp.

29: rotateDegree(rotate_degree) {}

◆ CbRotate() [2/2]

cl_nav2z::CbRotate::CbRotate ( float  rotate_degree,
cl_nav2z::SpinningPlanner  spinning_planner 
)

Definition at line 31 of file cb_rotate.cpp.

32: rotateDegree(rotate_degree), spinningPlanner(spinning_planner)
33{
34}
std::optional< cl_nav2z::SpinningPlanner > spinningPlanner
Definition: cb_rotate.hpp:35

Member Function Documentation

◆ onEntry()

void cl_nav2z::CbRotate::onEntry ( )
overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 36 of file cb_rotate.cpp.

37{
38 double angle_increment_degree = rotateDegree;
39
40 auto plannerSwitcher = nav2zClient_->getComponent<PlannerSwitcher>();
41
43 {
44 plannerSwitcher->setPureSpinningPlanner();
45 }
46 else
47 {
48 plannerSwitcher->setDefaultPlanners();
49 }
50
52 auto referenceFrame = p->getReferenceFrame();
53 auto currentPoseMsg = p->toPoseMsg();
54
55 tf2::Transform currentPose;
56 tf2::fromMsg(currentPoseMsg, currentPose);
57
58 auto odomTracker = nav2zClient_->getComponent<odom_tracker::OdomTracker>();
59 ClNav2Z::Goal goal;
60 goal.pose.header.frame_id = referenceFrame;
61 //goal.pose.header.stamp = getNode()->now();
62
63 auto currentAngle = tf2::getYaw(currentPoseMsg.orientation);
64 auto targetAngle = currentAngle + angle_increment_degree * M_PI / 180.0;
65 goal.pose.pose.position = currentPoseMsg.position;
66 tf2::Quaternion q;
67 q.setRPY(0, 0, targetAngle);
68 goal.pose.pose.orientation = tf2::toMsg(q);
69
70 geometry_msgs::msg::PoseStamped stampedCurrentPoseMsg;
71 stampedCurrentPoseMsg.header.frame_id = referenceFrame;
72 stampedCurrentPoseMsg.header.stamp = getNode()->now();
73 stampedCurrentPoseMsg.pose = currentPoseMsg;
74
76 auto pathname = this->getCurrentState()->getName() + " - " + getName();
77 odomTracker->pushPath(pathname);
78
79 odomTracker->setStartPoint(stampedCurrentPoseMsg);
80 odomTracker->setWorkingMode(odom_tracker::WorkingMode::RECORD_PATH);
81
82 RCLCPP_INFO_STREAM(getLogger(), "current pose: " << currentPoseMsg);
83 RCLCPP_INFO_STREAM(getLogger(), "goal pose: " << goal.pose.pose);
84 this->sendGoal(goal);
85}
const std::string & getReferenceFrame() const
Definition: cp_pose.hpp:76
virtual rclcpp::Logger getLogger() const
virtual rclcpp::Node::SharedPtr getNode() const
void requiresClient(SmaccClientType *&storage)
TComponent * getComponent()
virtual std::string getName()=0

References smacc2::ISmaccClient::getComponent(), smacc2::ISmaccClientBehavior::getCurrentState(), smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), smacc2::ISmaccState::getName(), smacc2::ISmaccClientBehavior::getNode(), cl_nav2z::Pose::getReferenceFrame(), cl_nav2z::CbNav2ZClientBehaviorBase::nav2zClient_, cl_nav2z::PureSpinning, cl_nav2z::odom_tracker::RECORD_PATH, smacc2::ISmaccClientBehavior::requiresClient(), rotateDegree, cl_nav2z::CbNav2ZClientBehaviorBase::sendGoal(), cl_nav2z::PlannerSwitcher::setPureSpinningPlanner(), and spinningPlanner.

Here is the call graph for this function:

Member Data Documentation

◆ goalChecker_

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

Definition at line 33 of file cb_rotate.hpp.

◆ listener

std::shared_ptr<tf2_ros::Buffer> cl_nav2z::CbRotate::listener
private

Definition at line 45 of file cb_rotate.hpp.

◆ rotateDegree

float cl_nav2z::CbRotate::rotateDegree

Definition at line 31 of file cb_rotate.hpp.

Referenced by onEntry().

◆ spinningPlanner

std::optional<cl_nav2z::SpinningPlanner> cl_nav2z::CbRotate::spinningPlanner

Definition at line 35 of file cb_rotate.hpp.

Referenced by onEntry().


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