#include <cb_keyboard_twist_teleop.hpp>

Inheritance diagram for cl_keyboard::CbKeyboardTwistTeleop:

Collaboration diagram for cl_keyboard::CbKeyboardTwistTeleop:

Public Member Functions
	CbKeyboardTwistTeleop (float linearStep=0.15f, float angularStep=0.4f, geometry_msgs::msg::Twist idleTwist=geometry_msgs::msg::Twist())

void	onEntry () override

void	onExit () override

void	update () override

Public Member Functions inherited from smacc2::SmaccClientBehavior
virtual	~SmaccClientBehavior ()

void	onEntry () override

void	onExit () override

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

virtual	~ISmaccClientBehavior ()

ISmaccStateMachine *	getStateMachine ()

std::string	getName () const

template<typename SmaccClientType >
void	requiresClient (SmaccClientType *&storage)

template<typename SmaccComponentType >
void	requiresComponent (SmaccComponentType *&storage, ComponentRequirement requirementType=ComponentRequirement::SOFT)

template<typename SmaccComponentType >
void	requiresComponent (std::string name, SmaccComponentType *&storage, ComponentRequirement requirementType=ComponentRequirement::SOFT)

virtual void	executeOnEntry ()

virtual void	executeOnExit ()

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

	ISmaccUpdatable (rclcpp::Duration duration)

void	executeUpdate (rclcpp::Node::SharedPtr node)

void	setUpdatePeriod (rclcpp::Duration duration)

Private Member Functions
void	onArrowPress (ArrowKey arrow)

Private Attributes
components::CpKeyboardListener1 *	keyboardListener_ = nullptr

smacc2::client_core_components::CpTopicPublisher< geometry_msgs::msg::Twist > *	twistPublisher_

float	linearStep_

float	angularStep_

geometry_msgs::msg::Twist	idleTwist_

std::mutex	mutex_

geometry_msgs::msg::Twist	heldTwist_

std::optional< std::chrono::steady_clock::time_point >	lastArrowTime_

std::chrono::milliseconds	deadman_ {350}

Additional Inherited Members
Protected Member Functions inherited from smacc2::ISmaccClientBehavior
virtual void	runtimeConfigure ()

template<typename EventType >
void	postEvent (const EventType &ev)

template<typename EventType >
void	postEvent ()

ISmaccState *	getCurrentState ()

virtual void	dispose ()

virtual rclcpp::Node::SharedPtr	getNode () const

virtual rclcpp::Logger	getLogger () const

Protected Member Functions inherited from smacc2::ISmaccUpdatable

Detailed Description

Definition at line 39 of file cb_keyboard_twist_teleop.hpp.

Constructor & Destructor Documentation

◆ CbKeyboardTwistTeleop()

cl_keyboard::CbKeyboardTwistTeleop::CbKeyboardTwistTeleop	(	float	linearStep = 0.15f,
		float	angularStep = 0.4f,
		geometry_msgs::msg::Twist	idleTwist = geometry_msgs::msg::Twist() )

inline

Definition at line 42 of file cb_keyboard_twist_teleop.hpp.

  : linearStep_(linearStep), angularStep_(angularStep), idleTwist_(idleTwist)
  {
  }

Member Function Documentation

◆ onArrowPress()

void cl_keyboard::CbKeyboardTwistTeleop::onArrowPress ( ArrowKey arrow )

inlineprivate

Definition at line 106 of file cb_keyboard_twist_teleop.hpp.

  {
    std::lock_guard<std::mutex> lock(mutex_);
    heldTwist_ = geometry_msgs::msg::Twist();
    switch (arrow)
    {
      case ArrowKey::Up:
        heldTwist_.linear.x = linearStep_;
        break;
      case ArrowKey::Down:
        heldTwist_.linear.x = -linearStep_;
        break;
      case ArrowKey::Left:
        heldTwist_.angular.z = angularStep_;
        break;
      case ArrowKey::Right:
        heldTwist_.angular.z = -angularStep_;
        break;
    }
    lastArrowTime_ = std::chrono::steady_clock::now();
  }

References angularStep_, cl_keyboard::Down, heldTwist_, lastArrowTime_, cl_keyboard::Left, linearStep_, mutex_, cl_keyboard::Right, and cl_keyboard::Up.

Referenced by onEntry().

Here is the caller graph for this function:

◆ onEntry()

void cl_keyboard::CbKeyboardTwistTeleop::onEntry ( )

inlineoverridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 49 of file cb_keyboard_twist_teleop.hpp.

  {
    // sync behavior: onEntry runs on the state machine thread, so component
    // resolution and signal wiring are safe here
    this->requiresComponent(keyboardListener_);
    this->requiresComponent(twistPublisher_);
 
    if (keyboardListener_ != nullptr)
    {
      keyboardListener_->OnArrowPress(&CbKeyboardTwistTeleop::onArrowPress, this);
    }
    else
    {
      RCLCPP_ERROR(getLogger(), "[%s] CpKeyboardListener1 not found", getName().c_str());
    }
 
    if (twistPublisher_ == nullptr)
    {
      RCLCPP_ERROR(
        getLogger(), "[%s] CpTopicPublisher<Twist> not found - create it in the orthogonal",
        getName().c_str());
    }
  }

References smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), keyboardListener_, cl_keyboard::components::CpKeyboardListener1::OnArrowPress(), onArrowPress(), smacc2::ISmaccClientBehavior::requiresComponent(), and twistPublisher_.

Here is the call graph for this function:

◆ onExit()

void cl_keyboard::CbKeyboardTwistTeleop::onExit ( )

inlineoverridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 73 of file cb_keyboard_twist_teleop.hpp.

  {
    // stop pushing: leave the topic at zero for whatever runs next
    if (twistPublisher_ != nullptr)
    {
      twistPublisher_->publish(geometry_msgs::msg::Twist());
    }
  }

References smacc2::client_core_components::CpTopicPublisher< MessageType >::publish(), and twistPublisher_.

Here is the call graph for this function:

◆ update()

void cl_keyboard::CbKeyboardTwistTeleop::update ( )

inlineoverridevirtual

Implements smacc2::ISmaccUpdatable.

Definition at line 82 of file cb_keyboard_twist_teleop.hpp.

  {
    if (twistPublisher_ == nullptr)
    {
      return;
    }
 
    geometry_msgs::msg::Twist out;
    bool held;
    {
      std::lock_guard<std::mutex> lock(mutex_);
      auto now = std::chrono::steady_clock::now();
      held = lastArrowTime_ && (now - *lastArrowTime_) < deadman_;
      out = held ? heldTwist_ : idleTwist_;
    }
    twistPublisher_->publish(out);
 
    RCLCPP_INFO_THROTTLE(
      getLogger(), *getNode()->get_clock(), 2000,
      "[%s] publishing %s twist: linear=%.2f angular=%.2f", getName().c_str(),
      held ? "held" : "idle", out.linear.x, out.angular.z);
  }