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
	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	onStateOrthogonalAllocation ()
template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()
virtual	~CbNav2ZClientBehaviorBase ()
template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()
template<typename TOrthogonal , typename TSourceObject >
void	onStateOrthogonalAllocation ()
Public Member Functions inherited from smacc2::SmaccAsyncClientBehavior
template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()
template<typename TOrthogonal , typename TSourceObject >
void	onStateOrthogonalAllocation ()
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)
template<typename SmaccComponentType >
void	requiresComponent (SmaccComponentType *&storage, ComponentRequirement requirementType=ComponentRequirement::SOFT)

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)
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 (nav2_msgs::action::NavigateToPose::Goal &goal)
void	cancelGoal ()
template<typename T >
boost::signals2::connection	onNavigationSucceeded (void(T::*callback)(const components::CpNav2ActionInterface::WrappedResult &), T *object)
template<typename T >
boost::signals2::connection	onNavigationAborted (void(T::*callback)(const components::CpNav2ActionInterface::WrappedResult &), T *object)
template<typename T >
boost::signals2::connection	onNavigationCancelled (void(T::*callback)(const components::CpNav2ActionInterface::WrappedResult &), T *object)
virtual void	onNavigationResult (const components::CpNav2ActionInterface::WrappedResult &)
virtual void	onNavigationActionSuccess (const components::CpNav2ActionInterface::WrappedResult &)
virtual void	onNavigationActionAbort (const components::CpNav2ActionInterface::WrappedResult &)
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
components::CpNav2ActionInterface *	nav2ActionInterface_ = nullptr
smacc2::client_core_components::CpActionClient< nav2_msgs::action::NavigateToPose > *	actionClient_ = nullptr
rclcpp_action::ResultCode	navigationResult_
cl_nav2z::ClNav2Z *	nav2zClient_
cl_nav2z::ClNav2Z::SmaccNavigateResultSignal::SharedPtr	navigationCallback_
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/4]

cl_nav2z::CbAbsoluteRotate::CbAbsoluteRotate

(

)

Definition at line 34 of file cb_absolute_rotate.cpp.

{}

CbAbsoluteRotate() [2/4]

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

Definition at line 35 of file cb_absolute_rotate.cpp.

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

References absoluteGoalAngleDegree, and yawGoalTolerance.

CbAbsoluteRotate() [3/4]

cl_nav2z::CbAbsoluteRotate::CbAbsoluteRotate

(

)

CbAbsoluteRotate() [4/4]

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

Member Function Documentation

onEntry() [1/2]

void cl_nav2z::CbAbsoluteRotate::onEntry ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Reimplemented in cl_nav2z::CbRotateLookAt, and cl_nav2z::CbRotateLookAt.

Definition at line 45 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);
 
  CpPlannerSwitcher * plannerSwitcher;
  this->requiresComponent(plannerSwitcher, ComponentRequirement::SOFT);
 
  if (spinningPlanner && *spinningPlanner == SpinningPlanner::PureSpinning)
  {
    plannerSwitcher->setPureSpinningPlanner();
  }
  else
  {
    plannerSwitcher->setDefaultPlanners();
  }
 
  updateTemporalBehaviorParameters(false);
 
  cl_nav2z::CpPose * p;
  this->requiresComponent(p, ComponentRequirement::HARD);
 
  auto referenceFrame = p->getReferenceFrame();
  auto currentPoseMsg = p->toPoseMsg();
 
  nav2_msgs::action::NavigateToPose::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);
 
  cl_nav2z::odom_tracker::CpOdomTracker * odomTracker_;
  this->requiresComponent(odomTracker_, ComponentRequirement::SOFT);
 
  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);
  }
 
  CpGoalCheckerSwitcher * goalCheckerSwitcher;
  this->requiresComponent(goalCheckerSwitcher, ComponentRequirement::HARD);
 
  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::ISmaccClientBehavior::getCurrentState(), smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), smacc2::ISmaccState::getName(), smacc2::ISmaccClientBehavior::getNode(), cl_nav2z::CpPose::getReferenceFrame(), listener, cl_nav2z::PureSpinning, cl_nav2z::odom_tracker::CpOdomTracker::pushPath(), cl_nav2z::odom_tracker::RECORD_PATH, smacc2::ISmaccClientBehavior::requiresComponent(), cl_nav2z::CbNav2ZClientBehaviorBase::sendGoal(), cl_nav2z::odom_tracker::CpOdomTracker::setCurrentMotionGoal(), cl_nav2z::CpPlannerSwitcher::setDefaultPlanners(), cl_nav2z::CpGoalCheckerSwitcher::setGoalCheckerId(), cl_nav2z::CpPlannerSwitcher::setPureSpinningPlanner(), cl_nav2z::odom_tracker::CpOdomTracker::setStartPoint(), cl_nav2z::odom_tracker::CpOdomTracker::setWorkingMode(), spinningPlanner, cl_nav2z::CpPose::toPoseMsg(), cl_nav2z::CpPose::toPoseStampedMsg(), and updateTemporalBehaviorParameters().

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

Here is the call graph for this function:

Here is the caller graph for this function:

onEntry() [2/2]

void cl_nav2z::CbAbsoluteRotate::onEntry ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Reimplemented in cl_nav2z::CbRotateLookAt, and cl_nav2z::CbRotateLookAt.

onExit() [1/2]

void cl_nav2z::CbAbsoluteRotate::onExit ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 265 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:

onExit() [2/2]

void cl_nav2z::CbAbsoluteRotate::onExit ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

updateTemporalBehaviorParameters() [1/2]

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

private

Definition at line 108 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:

updateTemporalBehaviorParameters() [2/2]

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

private

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/cl_nav2z/include/cl_nav2z/client_behaviors/cb_absolute_rotate.hpp
• smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_absolute_rotate.hpp
• smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_absolute_rotate.cpp
• smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_absolute_rotate.cpp