SMACC2
Public Member Functions | Public Attributes | List of all members
sm_dance_bot_warehouse_3::cl_nav2z::CpSquareShapeBoundary Class Reference

#include <cp_square_shape_bondary.hpp>

Inheritance diagram for sm_dance_bot_warehouse_3::cl_nav2z::CpSquareShapeBoundary:
Inheritance graph
Collaboration diagram for sm_dance_bot_warehouse_3::cl_nav2z::CpSquareShapeBoundary:
Collaboration graph

Public Member Functions

 CpSquareShapeBoundary (float squareLenghtMeters)
 
void onInitialize () override
 
float getForwardDistance ()
 
- Public Member Functions inherited from smacc2::ISmaccComponent
 ISmaccComponent ()
 
virtual ~ISmaccComponent ()
 
virtual std::string getName () const
 

Public Attributes

float squareLenghtMeters_
 
::cl_nav2z::PoserobotPose_
 

Additional Inherited Members

- Protected Member Functions inherited from smacc2::ISmaccComponent
virtual void onInitialize ()
 
template<typename EventType >
void postEvent (const EventType &ev)
 
template<typename EventType >
void postEvent ()
 
template<typename TOrthogonal , typename TSourceObject >
void onOrthogonalAllocation ()
 
template<typename TComponent >
void requiresComponent (TComponent *&requiredComponentStorage)
 
template<typename TClient >
void requiresClient (TClient *&requiredClientStorage)
 
template<typename SmaccComponentType , typename TOrthogonal , typename TClient , typename... TArgs>
SmaccComponentType * createSiblingComponent (TArgs... targs)
 
template<typename SmaccComponentType , typename TOrthogonal , typename TClient , typename... TArgs>
SmaccComponentType * createSiblingNamedComponent (std::string name, TArgs... targs)
 
rclcpp::Node::SharedPtr getNode ()
 
rclcpp::Logger getLogger ()
 
ISmaccStateMachinegetStateMachine ()
 
- Protected Attributes inherited from smacc2::ISmaccComponent
ISmaccStateMachinestateMachine_
 
ISmaccClientowner_
 

Detailed Description

Definition at line 35 of file cp_square_shape_bondary.hpp.

Constructor & Destructor Documentation

◆ CpSquareShapeBoundary()

sm_dance_bot_warehouse_3::cl_nav2z::CpSquareShapeBoundary::CpSquareShapeBoundary ( float  squareLenghtMeters)
inline

Definition at line 41 of file cp_square_shape_bondary.hpp.

Member Function Documentation

◆ getForwardDistance()

float sm_dance_bot_warehouse_3::cl_nav2z::CpSquareShapeBoundary::getForwardDistance ( )
inline

Definition at line 52 of file cp_square_shape_bondary.hpp.

53 {
54 float currentAngle = robotPose_->getYaw();
55
56 // warp 0 to 2pi
57 while(currentAngle < 0)
58 {
59 currentAngle+=2*M_PI;
60 }
61
62 float side = squareLenghtMeters_/2.0;
63
64 float fwdist = -1;
65 float x,y;
66
67 if((currentAngle > 0 && currentAngle < M_PI/4 ) // rightside
68 || (currentAngle > 7*M_PI/4 && currentAngle <= 2*M_PI) // rightside
69 || (currentAngle > 3*M_PI/4 && currentAngle <= 5*M_PI/4) // leftside
70 )
71 {
72 float m = tan(currentAngle);
73 x = side;
74 y = m*x;
75
76 fwdist = sqrt(x*x + y*y);
77 }
78 else if((currentAngle > M_PI/4 && currentAngle <= 3*M_PI/4) // top side
79 || (currentAngle > 5*M_PI/4 && currentAngle <= 7*M_PI/4)) // bottom side
80 {
81 float cotan = cos(currentAngle)/sin(currentAngle);
82 y = side;
83 x = cotan*y;
84
85 fwdist = sqrt(x*x + y*y);
86 }
87 else
88 {
89 RCLCPP_FATAL_STREAM(getLogger(), "[" << getName() << " ] not implemented case");
90 }
91
92 RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << " ] distance for yaw angle: " << currentAngle << " and square size: "<< squareLenghtMeters_ << " -- dist: " << fwdist <<"(x: "<< x <<" y:" << y <<")");
93
94 return fwdist;
95 }
float getYaw()
Definition: cp_pose.cpp:118
rclcpp::Logger getLogger()
virtual std::string getName() const

References smacc2::ISmaccComponent::getLogger(), smacc2::ISmaccComponent::getName(), cl_nav2z::Pose::getYaw(), robotPose_, and squareLenghtMeters_.

Here is the call graph for this function:

◆ onInitialize()

void sm_dance_bot_warehouse_3::cl_nav2z::CpSquareShapeBoundary::onInitialize ( )
inlineoverridevirtual

Reimplemented from smacc2::ISmaccComponent.

Definition at line 47 of file cp_square_shape_bondary.hpp.

48 {
50 }
void requiresComponent(TComponent *&requiredComponentStorage)

References smacc2::ISmaccComponent::requiresComponent(), and robotPose_.

Here is the call graph for this function:

Member Data Documentation

◆ robotPose_

::cl_nav2z::Pose* sm_dance_bot_warehouse_3::cl_nav2z::CpSquareShapeBoundary::robotPose_

Definition at line 39 of file cp_square_shape_bondary.hpp.

Referenced by getForwardDistance(), and onInitialize().

◆ squareLenghtMeters_

float sm_dance_bot_warehouse_3::cl_nav2z::CpSquareShapeBoundary::squareLenghtMeters_

Definition at line 38 of file cp_square_shape_bondary.hpp.

Referenced by getForwardDistance().


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