sm_autoware_avp::clients::ClAutoware Class Reference

#include <cl_autoware.hpp>

Inheritance diagram for sm_autoware_avp::clients::ClAutoware:

Collaboration diagram for sm_autoware_avp::clients::ClAutoware:

Public Member Functions
template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()
void	onInitialize () override
bool	checkGoalReached ()
void	onNdtPoseReceived (const geometry_msgs::msg::PoseWithCovarianceStamped &msg)
void	publishGoalPose (const geometry_msgs::msg::PoseStamped &msg)
void	publishInitialPoseEstimation (const geometry_msgs::msg::PoseWithCovarianceStamped &msg)
Public Member Functions inherited from smacc2::ISmaccClient
	ISmaccClient ()
virtual	~ISmaccClient ()
virtual void	onInitialize ()
virtual std::string	getName () const
template<typename TComponent >
TComponent *	getComponent ()
template<typename TComponent >
TComponent *	getComponent (std::string name)
template<typename TComponent >
TComponent *	getComponent (int index)
virtual smacc2::introspection::TypeInfo::Ptr	getType ()
ISmaccStateMachine *	getStateMachine ()
template<typename TSmaccSignal , typename T >
void	connectSignal (TSmaccSignal &signal, void(T::*callback)(), T *object)
template<typename SmaccClientType >
void	requiresClient (SmaccClientType *&storage)
void	getComponents (std::vector< std::shared_ptr< ISmaccComponent > > &components)
template<typename EventType >
void	postEvent (const EventType &ev)
template<typename EventType >
void	postEvent ()

Public Attributes
smacc2::components::CpTopicPublisher< geometry_msgs::msg::PoseWithCovarianceStamped > *	cppubLocation_
smacc2::components::CpTopicPublisher< geometry_msgs::msg::PoseStamped > *	cpppubGoalPose_
std::optional< geometry_msgs::msg::PoseWithCovarianceStamped >	lastPose_
std::optional< geometry_msgs::msg::PoseStamped >	lastGoal_
double	goalToleranceMeters_ = 3.5
std::function< void(const geometry_msgs::msg::PoseWithCovarianceStamped &)>	postEvAutoLocalized_
std::function< void()>	postEvGoalReached_

Additional Inherited Members
Protected Member Functions inherited from smacc2::ISmaccClient
template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()
template<typename SmaccComponentType , typename TOrthogonal , typename TClient , typename... TArgs>
SmaccComponentType *	createComponent (TArgs... targs)
template<typename SmaccComponentType , typename TOrthogonal , typename TClient , typename... TArgs>
SmaccComponentType *	createNamedComponent (std::string name, TArgs... targs)
rclcpp::Node::SharedPtr	getNode ()
rclcpp::Logger	getLogger ()
Protected Attributes inherited from smacc2::ISmaccClient
std::map< ComponentKey, std::shared_ptr< smacc2::ISmaccComponent > >	components_

Detailed Description

Definition at line 49 of file cl_autoware.hpp.

Member Function Documentation

checkGoalReached()

bool sm_autoware_avp::clients::ClAutoware::checkGoalReached ( )

inline

Definition at line 111 of file cl_autoware.hpp.

  {
    if (lastGoal_ && lastPose_)
    {
      auto & pos = this->lastPose_->pose.pose.position;
      auto & goalpos = this->lastGoal_->pose.position;
 
      double dx = pos.x - goalpos.x;
      double dy = pos.y - goalpos.y;
      double dz = pos.z - goalpos.z;
 
      double dist = sqrt(dx * dx + dy * dy + dz * dz);
 
      RCLCPP_INFO_STREAM_THROTTLE(getLogger(), *(getNode()->get_clock()),2000,  "[" << getName() << "] goal linear error: " << dist);
 
      if (dist < goalToleranceMeters_)
      {
        return true;
      }
    }
 
    return false;
  }

References smacc2::ISmaccClient::getLogger(), smacc2::ISmaccClient::getName(), smacc2::ISmaccClient::getNode(), goalToleranceMeters_, lastGoal_, and lastPose_.

Referenced by onNdtPoseReceived().

Here is the call graph for this function:

Here is the caller graph for this function:

onInitialize()

void sm_autoware_avp::clients::ClAutoware::onInitialize ( )

inlineoverridevirtual

Reimplemented from smacc2::ISmaccClient.

Definition at line 80 of file cl_autoware.hpp.

  {
    /*
      auto cppubLocation = client->createNamedComponent<
      smacc2::components::CpTopicPublisher<geometry_msgs::msg::PoseStamped>>(
      "initialPoseEstimation", "/localization/initialpose");
 
    auto cpppubGoalPose = client->createNamedComponent<
      smacc2::components::CpTopicPublisher<geometry_msgs::msg::PoseStamped>>(
      "goalPose", "planning/goal_pose");
    */
 
    cppubLocation_ =
      this->getComponent<smacc2::components::CpTopicPublisher<geometry_msgs::msg::PoseWithCovarianceStamped>>(
        "initialPoseEstimation");
 
    cpppubGoalPose_ =
      this->getComponent<smacc2::components::CpTopicPublisher<geometry_msgs::msg::PoseStamped>>(
        "goalPose");
 
    // auto subscriberNdtPose = client->createNamedComponent<
    //   smacc2::components::CpTopicSubscriber<geometry_msgs::msg::PoseWithCovarianceStamped>>(
    //   "ndtPose", "/localization/ndt_pose");
 
    auto subscriberNdtPose = this->getComponent<
      smacc2::components::CpTopicSubscriber<geometry_msgs::msg::PoseWithCovarianceStamped>>(
      "ndtPose");
 
    subscriberNdtPose->onMessageReceived(&ClAutoware::onNdtPoseReceived, this);
  }

References cpppubGoalPose_, cppubLocation_, smacc2::ISmaccClient::getComponent(), smacc2::components::CpTopicSubscriber< MessageType >::onMessageReceived(), and onNdtPoseReceived().

Here is the call graph for this function:

onNdtPoseReceived()

void sm_autoware_avp::clients::ClAutoware::onNdtPoseReceived ( const geometry_msgs::msg::PoseWithCovarianceStamped &  msg )

inline

Definition at line 135 of file cl_autoware.hpp.

  {
    lastPose_ = msg;
    this->postEvAutoLocalized_(msg);
 
    if(checkGoalReached())
    {
      this->postEvGoalReached_();
    }
  }

References checkGoalReached(), lastPose_, postEvAutoLocalized_, and postEvGoalReached_.

Referenced by onInitialize().

Here is the call graph for this function:

Here is the caller graph for this function:

onOrthogonalAllocation()

template<typename TOrthogonal , typename TSourceObject >

void sm_autoware_avp::clients::ClAutoware::onOrthogonalAllocation ( )

inline

Definition at line 65 of file cl_autoware.hpp.

  {
    this->postEvAutoLocalized_ = [this](const geometry_msgs::msg::PoseWithCovarianceStamped & msg)
    {
      auto ev = new EvAutoLocalized<TSourceObject, TOrthogonal>();
      ev->location = msg;
      this->postEvent(ev);
    };
 
    this->postEvGoalReached_ = [this]()
    {
      this->postEvent<EvGoalReached<TSourceObject, TOrthogonal>>();
    };
  }

References smacc2::ISmaccClient::postEvent().

Here is the call graph for this function:

publishGoalPose()

void sm_autoware_avp::clients::ClAutoware::publishGoalPose ( const geometry_msgs::msg::PoseStamped &  msg )

inline

Definition at line 146 of file cl_autoware.hpp.

  {
    lastGoal_ = msg;
    this->cpppubGoalPose_->publish(msg);
  }

References lastGoal_, and smacc2::components::CpTopicPublisher< MessageType >::publish().

Referenced by sm_autoware_avp::clients::autoware_client::CbNavigateGlobalPosition::onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

publishInitialPoseEstimation()

void sm_autoware_avp::clients::ClAutoware::publishInitialPoseEstimation ( const geometry_msgs::msg::PoseWithCovarianceStamped &  msg )

inline

Definition at line 152 of file cl_autoware.hpp.

  {
    this->cppubLocation_->publish(msg);
  }

References smacc2::components::CpTopicPublisher< MessageType >::publish().

Referenced by sm_autoware_avp::clients::autoware_client::CbSetupInitialPoseEstimation::onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

cpppubGoalPose_

smacc2::components::CpTopicPublisher<geometry_msgs::msg::PoseStamped>* sm_autoware_avp::clients::ClAutoware::cpppubGoalPose_

Definition at line 54 of file cl_autoware.hpp.

Referenced by onInitialize().

cppubLocation_

smacc2::components::CpTopicPublisher<geometry_msgs::msg::PoseWithCovarianceStamped>* sm_autoware_avp::clients::ClAutoware::cppubLocation_

Definition at line 52 of file cl_autoware.hpp.

Referenced by onInitialize().

goalToleranceMeters_

double sm_autoware_avp::clients::ClAutoware::goalToleranceMeters_ = 3.5

Definition at line 59 of file cl_autoware.hpp.

Referenced by checkGoalReached().

lastGoal_

std::optional<geometry_msgs::msg::PoseStamped> sm_autoware_avp::clients::ClAutoware::lastGoal_

Definition at line 57 of file cl_autoware.hpp.

Referenced by checkGoalReached(), and publishGoalPose().

lastPose_

std::optional<geometry_msgs::msg::PoseWithCovarianceStamped> sm_autoware_avp::clients::ClAutoware::lastPose_

Definition at line 56 of file cl_autoware.hpp.

Referenced by checkGoalReached(), and onNdtPoseReceived().

postEvAutoLocalized_

std::function<void(const geometry_msgs::msg::PoseWithCovarianceStamped &)> sm_autoware_avp::clients::ClAutoware::postEvAutoLocalized_

Definition at line 61 of file cl_autoware.hpp.

Referenced by onNdtPoseReceived().

postEvGoalReached_

std::function<void()> sm_autoware_avp::clients::ClAutoware::postEvGoalReached_

Definition at line 62 of file cl_autoware.hpp.

Referenced by onNdtPoseReceived().

---

The documentation for this class was generated from the following file:

• smacc2_sm_reference_library/sm_autoware_avp/include/sm_autoware_avp/clients/autoware_client/cl_autoware.hpp