#include <cb_absolute_rotate.hpp>

Inheritance diagram for cl_nav2z::CbAbsoluteRotate:

Collaboration diagram for cl_nav2z::CbAbsoluteRotate:

Public Member Functions
	CbAbsoluteRotate ()

	CbAbsoluteRotate (float absoluteGoalAngleDegree, float yawGoalTolerance=-1)

void	onEntry () override

void	onExit () override

Public Member Functions inherited from cl_nav2z::CbNav2ZClientBehaviorBase
virtual	~CbNav2ZClientBehaviorBase ()

template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()

Public Member Functions inherited from smacc2::SmaccAsyncClientBehavior
template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()

virtual	~SmaccAsyncClientBehavior ()

template<typename TCallback , typename T >
boost::signals2::connection	onSuccess (TCallback callback, T *object)

template<typename TCallback , typename T >
boost::signals2::connection	onFinished (TCallback callback, T *object)

template<typename TCallback , typename T >
boost::signals2::connection	onFailure (TCallback callback, T *object)

void	requestForceFinish ()

void	executeOnEntry () override

void	executeOnExit () override

void	waitOnEntryThread (bool requestFinish)

template<typename TCallbackMethod , typename T >
boost::signals2::connection	onSuccess (TCallbackMethod callback, T *object)

template<typename TCallbackMethod , typename T >
boost::signals2::connection	onFinished (TCallbackMethod callback, T *object)

template<typename TCallbackMethod , typename T >
boost::signals2::connection	onFailure (TCallbackMethod callback, T *object)

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, bool throwExceptionIfNotExist=false)

Public Attributes
std::shared_ptr< tf2_ros::Buffer >	listener

float	absoluteGoalAngleDegree

std::optional< float >	yawGoalTolerance

std::optional< float >	maxVelTheta

std::optional< SpinningPlanner >	spinningPlanner

Private Member Functions
void	updateTemporalBehaviorParameters (bool undo)

Private Attributes
float	oldYawTolerance

float	oldMaxVelTheta

float	oldMinVelTheta

Additional Inherited Members
Protected Member Functions inherited from cl_nav2z::CbNav2ZClientBehaviorBase
void	sendGoal (ClNav2Z::Goal &goal)

void	cancelGoal ()

bool	isOwnActionResponse (const ClNav2Z::WrappedResult &)

virtual void	onNavigationResult (const ClNav2Z::WrappedResult &)

virtual void	onNavigationActionSuccess (const ClNav2Z::WrappedResult &)

virtual void	onNavigationActionAbort (const ClNav2Z::WrappedResult &)

Protected Member Functions inherited from smacc2::SmaccAsyncClientBehavior
void	postSuccessEvent ()

void	postFailureEvent ()

virtual void	dispose () override

bool	isShutdownRequested ()
	onEntry is executed in a new thread. However the current state cannot be left until the onEntry thread finishes. This flag can be checked from the onEntry thread to force finishing the thread.

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 rclcpp::Node::SharedPtr	getNode () const

virtual rclcpp::Logger	getLogger () const

Protected Attributes inherited from cl_nav2z::CbNav2ZClientBehaviorBase
cl_nav2z::ClNav2Z *	nav2zClient_

cl_nav2z::ClNav2Z::SmaccNavigateResultSignal::SharedPtr	navigationCallback_

rclcpp_action::ResultCode	navigationResult_

std::shared_future< std::shared_ptr< rclcpp_action::ClientGoalHandle< nav2_msgs::action::NavigateToPose > > >	goalHandleFuture_

Detailed Description

Definition at line 29 of file cb_absolute_rotate.hpp.

Constructor & Destructor Documentation

◆ CbAbsoluteRotate() [1/2]

cl_nav2z::CbAbsoluteRotate::CbAbsoluteRotate ( )

Definition at line 32 of file cb_absolute_rotate.cpp.

32{}

◆ CbAbsoluteRotate() [2/2]

cl_nav2z::CbAbsoluteRotate::CbAbsoluteRotate	(	float	absoluteGoalAngleDegree,
		float	yawGoalTolerance = `-1`
	)

Definition at line 33 of file cb_absolute_rotate.cpp.

{
  this->absoluteGoalAngleDegree = absoluteGoalAngleDegree;
 
  if (yaw_goal_tolerance >= 0)
  {
    this->yawGoalTolerance = yaw_goal_tolerance;
  }
}

References absoluteGoalAngleDegree, and yawGoalTolerance.

Member Function Documentation

◆ onEntry()

void cl_nav2z::CbAbsoluteRotate::onEntry ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Reimplemented in cl_nav2z::CbRotateLookAt.

Definition at line 43 of file cb_absolute_rotate.cpp.

{
  listener = std::make_shared<tf2_ros::Buffer>(getNode()->get_clock());
  double goal_angle = this->absoluteGoalAngleDegree;
 
  RCLCPP_INFO_STREAM(getLogger(), "[CbAbsoluteRotate] Absolute yaw Angle:" << goal_angle);
 
  auto plannerSwitcher = this->nav2zClient_->getComponent<CpPlannerSwitcher>();
  // this should work better with a coroutine and await
  // this->plannerSwitcher_->setForwardPlanner();
 
  if (spinningPlanner && *spinningPlanner == SpinningPlanner::PureSpinning)
  {
    plannerSwitcher->setPureSpinningPlanner();
  }
  else
  {
    plannerSwitcher->setDefaultPlanners();
  }
 
  updateTemporalBehaviorParameters(false);
 
  auto p = nav2zClient_->getComponent<cl_nav2z::Pose>();
  auto referenceFrame = p->getReferenceFrame();
  auto currentPoseMsg = p->toPoseMsg();
 
  ClNav2Z::Goal goal;
  goal.pose.header.frame_id = referenceFrame;
  //goal.pose.header.stamp = getNode()->now();
 
  auto targetAngle = goal_angle * M_PI / 180.0;
  goal.pose.pose.position = currentPoseMsg.position;
  tf2::Quaternion q;
  q.setRPY(0, 0, targetAngle);
  goal.pose.pose.orientation = tf2::toMsg(q);
 
  auto odomTracker_ = nav2zClient_->getComponent<odom_tracker::CpOdomTracker>();
  if (odomTracker_ != nullptr)
  {
    auto pathname = this->getCurrentState()->getName() + " - " + getName();
    odomTracker_->pushPath(pathname);
    odomTracker_->setStartPoint(p->toPoseStampedMsg());
    odomTracker_->setCurrentMotionGoal(goal.pose);
    odomTracker_->setWorkingMode(odom_tracker::WorkingMode::RECORD_PATH);
  }
 
  auto goalCheckerSwitcher = nav2zClient_->getComponent<CpGoalCheckerSwitcher>();
  goalCheckerSwitcher->setGoalCheckerId("absolute_rotate_goal_checker");
 
  RCLCPP_INFO_STREAM(
    getLogger(),
    "[" << getName() << "] current pose yaw: " << tf2::getYaw(currentPoseMsg.orientation));
  RCLCPP_INFO_STREAM(
    getLogger(),
    "[" << getName() << "] goal pose yaw: " << tf2::getYaw(goal.pose.pose.orientation));
  this->sendGoal(goal);
}

References absoluteGoalAngleDegree, smacc2::ISmaccClient::getComponent(), smacc2::ISmaccClientBehavior::getCurrentState(), smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), smacc2::ISmaccState::getName(), smacc2::ISmaccClientBehavior::getNode(), cl_nav2z::Pose::getReferenceFrame(), listener, cl_nav2z::CbNav2ZClientBehaviorBase::nav2zClient_, cl_nav2z::PureSpinning, cl_nav2z::odom_tracker::RECORD_PATH, cl_nav2z::CbNav2ZClientBehaviorBase::sendGoal(), cl_nav2z::CpGoalCheckerSwitcher::setGoalCheckerId(), cl_nav2z::CpPlannerSwitcher::setPureSpinningPlanner(), spinningPlanner, and updateTemporalBehaviorParameters().

Referenced by cl_nav2z::CbRotateLookAt::onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ onExit()

void cl_nav2z::CbAbsoluteRotate::onExit ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 258 of file cb_absolute_rotate.cpp.

{
  if (spinningPlanner && *spinningPlanner == SpinningPlanner::PureSpinning)
  {
  }
  else
  {
  }
 
  this->updateTemporalBehaviorParameters(true);
}

References cl_nav2z::PureSpinning, spinningPlanner, and updateTemporalBehaviorParameters().

Here is the call graph for this function:

◆ updateTemporalBehaviorParameters()

void cl_nav2z::CbAbsoluteRotate::updateTemporalBehaviorParameters ( bool undo )

private

Definition at line 101 of file cb_absolute_rotate.cpp.

{
  auto log = this->getLogger();
 
  std::string nodename = "/controller_server";
 
  auto parameters_client =
    std::make_shared<rclcpp::AsyncParametersClient>(this->getNode(), nodename);
 
  RCLCPP_INFO_STREAM(
    log, "[" << getName() << "] using a parameter client to update some controller parameters: "
             << nodename << ". Waiting service.");
  parameters_client->wait_for_service();
 
  RCLCPP_INFO_STREAM(log, "[" << getName() << "] Service found: " << nodename << ".");
 
  std::string localPlannerName;
  std::vector<rclcpp::Parameter> parameters;
 
  // dynamic_reconfigure::DoubleParameter yaw_goal_tolerance;
  rclcpp::Parameter yaw_goal_tolerance("goal_checker.yaw_goal_tolerance");
 
  rclcpp::Parameter max_vel_theta, min_vel_theta;
 
  bool isRosBasePlanner = !spinningPlanner || *spinningPlanner == SpinningPlanner::Default;
  bool isPureSpinningPlanner = spinningPlanner && *spinningPlanner == SpinningPlanner::PureSpinning;
 
  // SELECTING CONTROLLER AND PARAMETERS NAME
  if (isPureSpinningPlanner)
  {
    localPlannerName = "PureSpinningLocalPlanner";
    max_vel_theta = rclcpp::Parameter(localPlannerName + ".max_angular_z_speed");
    min_vel_theta = rclcpp::Parameter(localPlannerName + ".min_vel_theta");
  }
  else if (isRosBasePlanner)
  {
    localPlannerName = "FollowPath";
    max_vel_theta = rclcpp::Parameter(localPlannerName + ".max_vel_theta");
    min_vel_theta = rclcpp::Parameter(localPlannerName + ".min_vel_theta");
  }
 
  if (!undo)
  {
    if (yawGoalTolerance)
    {
      // save old yaw tolerance
      auto fut = parameters_client->get_parameters(
        {localPlannerName + ".yaw_goal_tolerance"},
        [&](auto futureParameters)
        {
          auto params = futureParameters.get();
          oldYawTolerance = params[0].as_double();
        });
 
      // make synchronous
      fut.get();
 
      yaw_goal_tolerance = rclcpp::Parameter("goal_checker.yaw_goal_tolerance", *yawGoalTolerance);
      parameters.push_back(yaw_goal_tolerance);
      RCLCPP_INFO_STREAM(
        getLogger(), "[" << getName()
                         << "] updating yaw tolerance local planner to: " << *yawGoalTolerance
                         << ", from previous value: " << this->oldYawTolerance);
    }
 
    if (maxVelTheta)
    {
      if (isRosBasePlanner)
      {
        // nh.getParam(nodename + "/"  + localPlannerName+"/min_vel_theta", oldMinVelTheta);
        // getCurrentState()->getParam(nodename + "/" + localPlannerName + "/max_vel_theta", oldMaxVelTheta);
 
        // save old yaw tolerance
        auto fut = parameters_client->get_parameters(
          {localPlannerName + ".max_vel_theta"},
          [&](auto futureParameters)
          {
            auto params = futureParameters.get();
            oldMaxVelTheta = params[0].as_double();
          });
 
        // make synchronous
        fut.get();
      }
 
      max_vel_theta = rclcpp::Parameter(localPlannerName + ".max_vel_theta", *maxVelTheta);
      min_vel_theta = rclcpp::Parameter(localPlannerName + ".min_vel_theta", -*maxVelTheta);
      parameters.push_back(max_vel_theta);
      parameters.push_back(min_vel_theta);
 
      RCLCPP_INFO_STREAM(
        log, "[" << getName() << "] updating max vel theta local planner to: " << *maxVelTheta
                 << ", from previous value: " << this->oldMaxVelTheta);
      RCLCPP_INFO_STREAM(
        log, "[" << getName() << "] updating min vel theta local planner to: " << -*maxVelTheta
                 << ", from previous value: " << this->oldMinVelTheta);
    }
  }
  else
  {
    if (yawGoalTolerance)
    {
      yaw_goal_tolerance = rclcpp::Parameter("goal_checker.yaw_goal_tolerance", oldYawTolerance);
      RCLCPP_INFO_STREAM(
        log, "[" << getName() << "] restoring yaw tolerance local planner from: "
                 << *yawGoalTolerance << " , to previous value: " << this->oldYawTolerance);
    }
 
    if (maxVelTheta)
    {
      if (isRosBasePlanner)
      {
        max_vel_theta = rclcpp::Parameter(localPlannerName + ".max_vel_theta", oldMaxVelTheta);
        min_vel_theta = rclcpp::Parameter(localPlannerName + ".max_vel_theta", oldMinVelTheta);
      }
 
      parameters.push_back(max_vel_theta);
      parameters.push_back(min_vel_theta);
      RCLCPP_INFO_STREAM(
        log, "[" << getName() << "] restoring max vel theta local planner from: " << *maxVelTheta
                 << ", to previous value: " << this->oldMaxVelTheta);
      RCLCPP_INFO_STREAM(
        log, "[" << getName() << "] restoring min vel theta local planner from: " << -(*maxVelTheta)
                 << ", to previous value: " << this->oldMinVelTheta);
    }
  }
 
  if (parameters.size())
  {
    std::stringstream ss;
    ss << "Executing asynchronous request. Parameters to update: " << std::endl;
    for (auto & p : parameters)
    {
      ss << p.get_name() << " - " << p.value_to_string() << std::endl;
    }
 
    RCLCPP_INFO_STREAM(getLogger(), "[CbAbsoluteRotate] " << ss.str());
 
    auto futureResults = parameters_client->set_parameters(parameters);
 
    int i = 0;
    for (auto & res : futureResults.get())
    {
      RCLCPP_INFO_STREAM(
        getLogger(), "[" << getName() << "] parameter result: " << parameters[i].get_name() << "="
                         << parameters[i].as_string() << ". Result: " << res.successful);
      i++;
    }
 
    RCLCPP_INFO_STREAM(log, "[" << getName() << "] parameters updated");
  }
  else
  {
    RCLCPP_INFO_STREAM(log, "[" << getName() << "] skipping parameters update");
  }
}

References cl_nav2z::Default, smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), smacc2::ISmaccClientBehavior::getNode(), maxVelTheta, oldMaxVelTheta, oldMinVelTheta, oldYawTolerance, cl_nav2z::PureSpinning, spinningPlanner, and yawGoalTolerance.

Referenced by onEntry(), and onExit().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ absoluteGoalAngleDegree

float cl_nav2z::CbAbsoluteRotate::absoluteGoalAngleDegree

Definition at line 34 of file cb_absolute_rotate.hpp.

Referenced by CbAbsoluteRotate(), onEntry(), and cl_nav2z::CbRotateLookAt::onEntry().

◆ listener

std::shared_ptr<tf2_ros::Buffer> cl_nav2z::CbAbsoluteRotate::listener

Definition at line 32 of file cb_absolute_rotate.hpp.

Referenced by onEntry().

◆ maxVelTheta

std::optional<float> cl_nav2z::CbAbsoluteRotate::maxVelTheta

Definition at line 36 of file cb_absolute_rotate.hpp.

Referenced by updateTemporalBehaviorParameters().

◆ oldMaxVelTheta

float cl_nav2z::CbAbsoluteRotate::oldMaxVelTheta

private

Definition at line 48 of file cb_absolute_rotate.hpp.

Referenced by updateTemporalBehaviorParameters().

◆ oldMinVelTheta

float cl_nav2z::CbAbsoluteRotate::oldMinVelTheta

private

Definition at line 49 of file cb_absolute_rotate.hpp.

Referenced by updateTemporalBehaviorParameters().

◆ oldYawTolerance

float cl_nav2z::CbAbsoluteRotate::oldYawTolerance

private

Definition at line 47 of file cb_absolute_rotate.hpp.

Referenced by updateTemporalBehaviorParameters().

◆ spinningPlanner

std::optional<SpinningPlanner> cl_nav2z::CbAbsoluteRotate::spinningPlanner

Definition at line 37 of file cb_absolute_rotate.hpp.

Referenced by onEntry(), onExit(), and updateTemporalBehaviorParameters().

◆ yawGoalTolerance

std::optional<float> cl_nav2z::CbAbsoluteRotate::yawGoalTolerance

Definition at line 35 of file cb_absolute_rotate.hpp.

Referenced by CbAbsoluteRotate(), and updateTemporalBehaviorParameters().

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

smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_absolute_rotate.hpp
smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_absolute_rotate.cpp

Public Member Functions

Public Attributes

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ CbAbsoluteRotate() [1/2]

◆ CbAbsoluteRotate() [2/2]

Member Function Documentation

◆ onEntry()

◆ onExit()

◆ updateTemporalBehaviorParameters()

Member Data Documentation

◆ absoluteGoalAngleDegree

◆ listener

◆ maxVelTheta

◆ oldMaxVelTheta

◆ oldMinVelTheta

◆ oldYawTolerance

◆ spinningPlanner

◆ yawGoalTolerance