28using namespace std::chrono_literals;
51 m = (b < 0) ? m - b : m + b;
65 for (
int i = 0; i < raysWidthCount; i++)
68 int scanindex = (- raysWidthCount / 2 + i) % raysWidthCount;
79 <<
"range[" << scanindex <<
"] = " << fwdist2);
81 if (fwdist2 > 0.01 && fwdist2 < fwdist)
85 <<
"updated range[0] = " << fwdist2);
94 if (fwdist == std::numeric_limits<float>::infinity() || fwdist != fwdist)
99 "[CpForwardObstacleDetector] Distance to forward obstacle is not a number, setting default value "
105 RCLCPP_INFO_STREAM(
getLogger(),
"[" <<
getName() <<
"] fwdist: " << fwdist);
107 RCLCPP_INFO_STREAM(
getLogger(),
"[" <<
getName() <<
"] fwdist: " << fwdist);
115 this->lastScanMessage_ = scanmsg;
int modulo_Euclidean(int a, int b)
sensor_msgs::msg::LaserScan lastScanMessage_
void onInitialize() override
void MessageCallbackStoreDistanceToWall(const sensor_msgs::msg::LaserScan &scanmsg)
float getForwardDistance(int raysWidthCount=0)
const float SECURITY_DISTANCE
rclcpp::Logger getLogger()
virtual std::string getName() const
rclcpp::Node::SharedPtr getNode()
boost::signals2::connection onMessageReceived(void(T::*callback)(const MessageType &), T *object)