#include <cp_forward_obstacle_detector.hpp>

Inheritance diagram for sm_dance_bot_warehouse_2::cl_lidar::CpForwardObstacleDetector:

Collaboration diagram for sm_dance_bot_warehouse_2::cl_lidar::CpForwardObstacleDetector:

Public Member Functions
void	onInitialize () override

int	modulo_Euclidean (int a, int b)

float	getForwardDistance (int raysWidthCount=0)

void	MessageCallbackStoreDistanceToWall (const sensor_msgs::msg::LaserScan &scanmsg)

Public Member Functions inherited from smacc2::ISmaccComponent
	ISmaccComponent ()

virtual	~ISmaccComponent ()

virtual std::string	getName () const

Public Attributes
sensor_msgs::msg::LaserScan	lastScanMessage_

const float	SECURITY_DISTANCE = 0.4

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 ()

ISmaccStateMachine *	getStateMachine ()

Protected Attributes inherited from smacc2::ISmaccComponent
ISmaccStateMachine *	stateMachine_

ISmaccClient *	owner_

Detailed Description

Definition at line 30 of file cp_forward_obstacle_detector.hpp.

Member Function Documentation

◆ getForwardDistance()

float sm_dance_bot_warehouse_2::cl_lidar::CpForwardObstacleDetector::getForwardDistance ( int raysWidthCount = 0 )

inline

Definition at line 56 of file cp_forward_obstacle_detector.hpp.

  {
    //auto fwdist = scanmsg.ranges[scanmsg.ranges.size() / 2] /*meter*/;
    auto fwdist = lastScanMessage_.ranges[0] /*meter*/;
 
//     RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] ranges[0]" << ss.str());
 
    std::stringstream ss;
    //check rays around main ray
    for (int i = 0; i < raysWidthCount; i++)
    {
      // int baseindex = 0;
      int scanindex =  (- raysWidthCount / 2 + i) % raysWidthCount;
 
      if(scanindex < 0)
      {
        scanindex = lastScanMessage_.ranges.size() + scanindex;
      }
 
      float fwdist2 = lastScanMessage_.ranges[scanindex];
 
      RCLCPP_INFO_STREAM(
        getLogger(), "[" << getName() << "]"
                         << "range[" << scanindex << "] = " << fwdist2);
 
      if (fwdist2 > 0.01 && fwdist2 < fwdist)
      {
        RCLCPP_INFO_STREAM(
          getLogger(), "[" << getName() << "]"
                           << "updated range[0] = " << fwdist2);
 
        fwdist = fwdist2;
      }
    }
 
    RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "]" << ss.str());
 
    /*if the distance is infinity or nan, use max range distance*/
    if (fwdist == std::numeric_limits<float>::infinity() || fwdist != fwdist)
    {
      fwdist = lastScanMessage_.range_max - SECURITY_DISTANCE /*meters*/;
      RCLCPP_INFO_THROTTLE(
        getLogger(), *(getNode()->get_clock()), 1000,
        "[CpForwardObstacleDetector] Distance to forward obstacle is not a number, setting default value "
        "to: %lf",
        lastScanMessage_.range_max);
    }
    else
    {
      RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] fwdist: " << fwdist);
      fwdist = std::max(fwdist - SECURITY_DISTANCE, 0.0F);
      RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] fwdist: " << fwdist);
    }
 
    return fwdist;
  }

References smacc2::ISmaccComponent::getLogger(), smacc2::ISmaccComponent::getName(), smacc2::ISmaccComponent::getNode(), lastScanMessage_, and SECURITY_DISTANCE.

Referenced by sm_dance_bot_warehouse_2::s_pattern_states::StiSPatternForward1::runtimeConfigure(), sm_dance_bot_warehouse_2::s_pattern_states::StiSPatternForward2::runtimeConfigure(), sm_dance_bot_warehouse_2::s_pattern_states::StiSPatternForward3::runtimeConfigure(), and sm_dance_bot_warehouse_2::s_pattern_states::StiSPatternForward4::runtimeConfigure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ MessageCallbackStoreDistanceToWall()

void sm_dance_bot_warehouse_2::cl_lidar::CpForwardObstacleDetector::MessageCallbackStoreDistanceToWall ( const sensor_msgs::msg::LaserScan & scanmsg )

inline

Definition at line 113 of file cp_forward_obstacle_detector.hpp.

  {
    this->lastScanMessage_ = scanmsg;
  }

Referenced by onInitialize().

Here is the caller graph for this function:

◆ modulo_Euclidean()

int sm_dance_bot_warehouse_2::cl_lidar::CpForwardObstacleDetector::modulo_Euclidean	(	int	a,
		int	b
	)

inline

Definition at line 45 of file cp_forward_obstacle_detector.hpp.

  {
    int m = a % b;
    if (m < 0)
    {
      // m += (b < 0) ? -b : b; // avoid this form: -b is UB when b == INT_MIN
      m = (b < 0) ? m - b : m + b;
    }
    return m;
  }

◆ onInitialize()

void sm_dance_bot_warehouse_2::cl_lidar::CpForwardObstacleDetector::onInitialize ( )

inlineoverridevirtual

Reimplemented from smacc2::ISmaccComponent.

Definition at line 37 of file cp_forward_obstacle_detector.hpp.

  {
    auto client_ =
      dynamic_cast<smacc2::client_bases::SmaccSubscriberClient<sensor_msgs::msg::LaserScan> *>(
        owner_);
    client_->onMessageReceived(&CpForwardObstacleDetector::MessageCallbackStoreDistanceToWall, this);
  }