31using namespace std::chrono_literals;
57 while(currentAngle < 0)
67 if((currentAngle > 0 && currentAngle < M_PI/4 )
68 || (currentAngle > 7*M_PI/4 && currentAngle <= 2*M_PI)
69 || (currentAngle > 3*M_PI/4 && currentAngle <= 5*M_PI/4)
72 float m = tan(currentAngle);
76 fwdist = sqrt(x*x + y*y);
78 else if((currentAngle > M_PI/4 && currentAngle <= 3*M_PI/4)
79 || (currentAngle > 5*M_PI/4 && currentAngle <= 7*M_PI/4))
81 float cotan = cos(currentAngle)/sin(currentAngle);
85 fwdist = sqrt(x*x + y*y);
89 RCLCPP_FATAL_STREAM(
getLogger(),
"[" <<
getName() <<
" ] not implemented case");
92 RCLCPP_INFO_STREAM(
getLogger(),
"[" <<
getName() <<
" ] distance for yaw angle: " << currentAngle <<
" and square size: "<<
squareLenghtMeters_ <<
" -- dist: " << fwdist <<
"(x: "<< x <<
" y:" << y <<
")");
void onInitialize() override
float getForwardDistance()
float squareLenghtMeters_
::cl_nav2z::Pose * robotPose_
CpSquareShapeBoundary(float squareLenghtMeters)
void requiresComponent(TComponent *&requiredComponentStorage)
rclcpp::Logger getLogger()
virtual std::string getName() const