cl_nav2z::CbAbsoluteRotate Class Reference

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

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 smacc2::SmaccAsyncClientBehavior
void	postSuccessEvent ()
void	postFailureEvent ()
virtual void	dispose () override
bool	isShutdownRequested ()
Protected Member Functions inherited from smacc2::ISmaccClientBehavior
virtual void	runtimeConfigure ()
virtual void	onEntry ()
virtual void	onExit ()
template<typename EventType >
void	postEvent (const EventType &ev)
template<typename EventType >
void	postEvent ()
ISmaccState *	getCurrentState ()
virtual void	dispose ()
virtual rclcpp::Node::SharedPtr	getNode ()
virtual rclcpp::Logger	getLogger ()
Protected Attributes inherited from cl_nav2z::CbNav2ZClientBehaviorBase
cl_nav2z::ClNav2Z *	moveBaseClient_
rclcpp_action::ResultCode	navigationResult_

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.

{}

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.

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->moveBaseClient_->getComponent<PlannerSwitcher>();
  // 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 = moveBaseClient_->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_ = moveBaseClient_->getComponent<odom_tracker::OdomTracker>();
  if (odomTracker_ != nullptr)
  {
    auto pathname = this->getCurrentState()->getName() + " - " + getName();
    odomTracker_->pushPath(pathname);
    odomTracker_->setStartPoint(p->toPoseStampedMsg());
    odomTracker_->setWorkingMode(odom_tracker::WorkingMode::RECORD_PATH);
  }
 
  auto goalCheckerSwitcher = moveBaseClient_->getComponent<GoalCheckerSwitcher>();
  goalCheckerSwitcher->setGoalCheckerId("absolute_rotate_goal_checker");
 
  RCLCPP_INFO_STREAM(
    getLogger(),
    "[CbAbsoluteRotate] current pose yaw: " << tf2::getYaw(currentPoseMsg.orientation));
  RCLCPP_INFO_STREAM(
    getLogger(), "[CbAbsoluteRotate] goal pose yaw: " << tf2::getYaw(goal.pose.pose.orientation));
  moveBaseClient_->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::moveBaseClient_, cl_nav2z::PureSpinning, cl_nav2z::odom_tracker::RECORD_PATH, smacc2::client_bases::SmaccActionClientBase< ActionType >::sendGoal(), cl_nav2z::GoalCheckerSwitcher::setGoalCheckerId(), cl_nav2z::PlannerSwitcher::setPureSpinningPlanner(), spinningPlanner, and updateTemporalBehaviorParameters().

Here is the call graph for this function:

onExit()

void cl_nav2z::CbAbsoluteRotate::onExit ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 264 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 99 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, "[CbAbsoluteRotate] using a parameter client to update some controller parameters: "
           << nodename << ". Waiting service.");
  parameters_client->wait_for_service();
 
  RCLCPP_INFO_STREAM(log, "[CbAbsoluteRotate] 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(
        getLogger(),
        "[CbAbsoluteRotate] updating yaw tolerance local planner to: %lf, from previous value: "
        "%lf ",
        *yawGoalTolerance, 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(
        log,
        "[CbAbsoluteRotate] updating max vel theta local planner to: %lf, from previous value: "
        "%lf ",
        *maxVelTheta, this->oldMaxVelTheta);
      RCLCPP_INFO(
        log,
        "[CbAbsoluteRotate] updating min vel theta local planner to: %lf, from previous value: "
        "%lf ",
        -*maxVelTheta, this->oldMinVelTheta);
    }
  }
  else
  {
    if (yawGoalTolerance)
    {
      yaw_goal_tolerance = rclcpp::Parameter("goal_checker.yaw_goal_tolerance", oldYawTolerance);
      RCLCPP_INFO(
        log,
        "[CbAbsoluteRotate] restoring yaw tolerance local planner from: %lf, to previous value: "
        "%lf ",
        *yawGoalTolerance, 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(
        log,
        "[CbAbsoluteRotate] restoring max vel theta local planner from: %lf, to previous value: "
        "%lf ",
        *maxVelTheta, this->oldMaxVelTheta);
      RCLCPP_INFO(
        log,
        "[CbAbsoluteRotate] restoring min vel theta local planner from: %lf, to previous value: "
        "%lf ",
        -(*maxVelTheta), 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(), "[CbAbsoluteRotate] parameter result: " << parameters[i].get_name() << "="
                                                             << parameters[i].as_string()
                                                             << ". Result: " << res.successful);
      i++;
    }
 
    RCLCPP_INFO(log, "[CbAbsoluteRotate] parameters updated");
  }
  else
  {
    RCLCPP_INFO(log, "[CbAbsoluteRotate] skipping parameters update");
  }
}

References cl_nav2z::Default, smacc2::ISmaccClientBehavior::getLogger(), 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(), and 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(), sm_dance_bot::radial_motion_states::StiRadialRotate::runtimeConfigure(), sm_dance_bot_strikes_back::radial_motion_states::StiRadialRotate::runtimeConfigure(), 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