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

#include <cp_square_shape_bondary.hpp>

Inheritance diagram for sm_dance_bot_warehouse::cl_nav2z::CpSquareShapeBoundary:
Inheritance graph
Collaboration diagram for sm_dance_bot_warehouse::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::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::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 // float maxraysize = sqrt(2*side*side);
64 // float radius = maxraysize;
65
66 float fwdist = -1;
67 float x,y;
68
69 if((currentAngle > 0 && currentAngle < M_PI/4 ) // rightside
70 || (currentAngle > 7*M_PI/4 && currentAngle <= 2*M_PI) // rightside
71 || (currentAngle > 3*M_PI/4 && currentAngle <= 5*M_PI/4) // leftside
72 )
73 {
74 float m = tan(currentAngle);
75 x = side;
76 y = m*x;
77
78 fwdist = sqrt(x*x + y*y);
79 }
80 else if((currentAngle > M_PI/4 && currentAngle <= 3*M_PI/4) // top side
81 || (currentAngle > 5*M_PI/4 && currentAngle <= 7*M_PI/4)) // bottom side
82 {
83 float cotan = cos(currentAngle)/sin(currentAngle);
84 y = side;
85 x = cotan*y;
86
87 fwdist = sqrt(x*x + y*y);
88 }
89 else
90 {
91 RCLCPP_FATAL_STREAM(getLogger(), "[" << getName() << " ] not implemented case");
92 }
93
94 RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << " ] distance for yaw angle: " << currentAngle << " and square size: "<< squareLenghtMeters_ << " -- dist: " << fwdist <<"(x: "<< x <<" y:" << y <<")");
95
96 return fwdist;
97 }
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::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::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::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: