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