cl_move_base_z::CbAbsoluteRotate Class Reference

#include <cb_absolute_rotate.h>

Inheritance diagram for cl_move_base_z::CbAbsoluteRotate:

Collaboration diagram for cl_move_base_z::CbAbsoluteRotate:

Public Member Functions
	CbAbsoluteRotate ()
	CbAbsoluteRotate (float absoluteGoalAngleDegree, float yawGoalTolerance=-1)
virtual void	onEntry () override
virtual void	onExit () override
Public Member Functions inherited from cl_move_base_z::CbMoveBaseClientBehaviorBase
template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()
Public Member Functions inherited from smacc::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 smacc::ISmaccClientBehavior
	ISmaccClientBehavior ()
virtual	~ISmaccClientBehavior ()
ISmaccStateMachine *	getStateMachine ()
std::string	getName () const
template<typename SmaccClientType >
void	requiresClient (SmaccClientType *&storage)
template<typename SmaccComponentType >
void	requiresComponent (SmaccComponentType *&storage)
ros::NodeHandle	getNode ()

Public Attributes
tf::TransformListener	listener
boost::optional< float >	absoluteGoalAngleDegree
boost::optional< float >	yawGoalTolerance
boost::optional< float >	maxVelTheta
boost::optional< SpiningPlanner >	spinningPlanner

Private Member Functions
void	updateTemporalBehaviorParameters (bool undo)

Private Attributes
float	oldYawTolerance
float	oldMaxVelTheta
float	oldMinVelTheta

Additional Inherited Members
Protected Member Functions inherited from smacc::SmaccAsyncClientBehavior
virtual void	executeOnEntry () override
virtual void	executeOnExit () override
void	postSuccessEvent ()
void	postFailureEvent ()
virtual void	dispose () override
Protected Member Functions inherited from smacc::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	executeOnEntry ()
virtual void	executeOnExit ()
virtual void	dispose ()
Protected Attributes inherited from cl_move_base_z::CbMoveBaseClientBehaviorBase
ClMoveBaseZ *	moveBaseClient_
ros::Publisher	visualizationMarkersPub_

Detailed Description

Definition at line 21 of file cb_absolute_rotate.h.

Constructor & Destructor Documentation

CbAbsoluteRotate() [1/2]

cl_move_base_z::CbAbsoluteRotate::CbAbsoluteRotate

(

)

Definition at line 8 of file cb_absolute_rotate.cpp.

{
}

CbAbsoluteRotate() [2/2]

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

Definition at line 12 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_move_base_z::CbAbsoluteRotate::onEntry ( )

overridevirtual

Reimplemented from smacc::ISmaccClientBehavior.

Definition at line 137 of file cb_absolute_rotate.cpp.

{
  double goal_angle;
  ROS_INFO_STREAM("[CbAbsoluteRotate] Absolute yaw Angle:" << this->absoluteGoalAngleDegree);
 
  if (!this->absoluteGoalAngleDegree)
  {
    this->getCurrentState()->param("goal_angle", goal_angle, 45.0);
  }
  else
  {
    goal_angle = *this->absoluteGoalAngleDegree;
  }
 
  auto plannerSwitcher = moveBaseClient_->getComponent<PlannerSwitcher>();
  // this should work better with a coroutine and await
  // this->plannerSwitcher_->setForwardPlanner();
 
  if (spinningPlanner && *spinningPlanner == SpiningPlanner::PureSpinning)
    plannerSwitcher->setPureSpinningPlanner();
  else
    plannerSwitcher->setDefaultPlanners();
 
  updateTemporalBehaviorParameters(false);
 
  auto p = moveBaseClient_->getComponent<cl_move_base_z::Pose>();
  auto referenceFrame = p->getReferenceFrame();
  auto currentPoseMsg = p->toPoseMsg();
 
  ClMoveBaseZ::Goal goal;
  goal.target_pose.header.frame_id = referenceFrame;
  goal.target_pose.header.stamp = ros::Time::now();
 
  auto currentAngle = tf::getYaw(currentPoseMsg.orientation);
  auto targetAngle = goal_angle * M_PI / 180.0;
  goal.target_pose.pose.position = currentPoseMsg.position;
  goal.target_pose.pose.orientation = tf::createQuaternionMsgFromYaw(targetAngle);
 
  auto odomTracker_ = moveBaseClient_->getComponent<odom_tracker::OdomTracker>();
  if (odomTracker_ != nullptr)
  {
    odomTracker_->pushPath("PureSpinningToAbsoluteGoalOrientation");
    odomTracker_->setStartPoint(p->toPoseStampedMsg());
    odomTracker_->setWorkingMode(odom_tracker::WorkingMode::RECORD_PATH);
  }
 
  ROS_INFO_STREAM("[CbAbsoluteRotate] current pose: " << currentPoseMsg);
  ROS_INFO_STREAM("[CbAbsoluteRotate] goal pose: " << goal.target_pose.pose);
  moveBaseClient_->sendGoal(goal);
}

References absoluteGoalAngleDegree, smacc::ISmaccClient::getComponent(), smacc::ISmaccClientBehavior::getCurrentState(), cl_move_base_z::Pose::getReferenceFrame(), cl_move_base_z::CbMoveBaseClientBehaviorBase::moveBaseClient_, smacc::ISmaccState::param(), cl_move_base_z::PureSpinning, cl_move_base_z::odom_tracker::OdomTracker::pushPath(), cl_move_base_z::odom_tracker::RECORD_PATH, smacc::client_bases::SmaccActionClientBase< ActionType >::sendGoal(), cl_move_base_z::PlannerSwitcher::setPureSpinningPlanner(), spinningPlanner, and updateTemporalBehaviorParameters().

Here is the call graph for this function:

onExit()

void cl_move_base_z::CbAbsoluteRotate::onExit ( )

overridevirtual

Reimplemented from smacc::ISmaccClientBehavior.

Definition at line 125 of file cb_absolute_rotate.cpp.

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

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

Here is the call graph for this function:

updateTemporalBehaviorParameters()

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

private

Definition at line 22 of file cb_absolute_rotate.cpp.

{
  dynamic_reconfigure::ReconfigureRequest srv_req;
  dynamic_reconfigure::ReconfigureResponse srv_resp;
  dynamic_reconfigure::Config conf;
 
  ros::NodeHandle nh;
  std::string nodename = "/move_base";
  std::string localPlannerName;
  dynamic_reconfigure::DoubleParameter yaw_goal_tolerance;
  yaw_goal_tolerance.name = "yaw_goal_tolerance";
 
  dynamic_reconfigure::DoubleParameter max_vel_theta;
  dynamic_reconfigure::DoubleParameter min_vel_theta;
  bool isRosBasePlanner = !spinningPlanner || *spinningPlanner == SpiningPlanner::Default;
  bool isPureSpinningPlanner = spinningPlanner && *spinningPlanner == SpiningPlanner::PureSpinning;
 
  if (isPureSpinningPlanner)
  {
    localPlannerName = "PureSpinningLocalPlanner";
    max_vel_theta.name = "max_angular_z_speed";
    min_vel_theta.name = "min_vel_theta";
  }
  else if (isRosBasePlanner)
  {
    localPlannerName = "TrajectoryPlannerROS";
    max_vel_theta.name = "max_vel_theta";
    min_vel_theta.name = "min_vel_theta";
  }
 
  if (!undo)
  {
    if (yawGoalTolerance)
    {
      // save old yaw tolerance
      nh.getParam(nodename + "/" + localPlannerName + "/yaw_goal_tolerance", oldYawTolerance);
      yaw_goal_tolerance.value = *yawGoalTolerance;
      conf.doubles.push_back(yaw_goal_tolerance);
      ROS_INFO("[CbAbsoluteRotate] updating yaw tolerance local planner to: %lf, from previous value: %lf ",
               *yawGoalTolerance, this->oldYawTolerance);
    }
 
    if (maxVelTheta)
    {
      if (isRosBasePlanner)
      {
        // save old yaw tolerance
        // nh.getParam(nodename + "/"  + localPlannerName+"/min_vel_theta", oldMinVelTheta);
        nh.getParam(nodename + "/" + localPlannerName + "/max_vel_theta", oldMaxVelTheta);
      }
      max_vel_theta.value = *maxVelTheta;
      min_vel_theta.value = -*maxVelTheta;
      conf.doubles.push_back(max_vel_theta);
      conf.doubles.push_back(min_vel_theta);
      ROS_INFO("[CbAbsoluteRotate] updating max vel theta local planner to: %lf, from previous value: %lf ",
               *maxVelTheta, this->oldMaxVelTheta);
      ROS_INFO("[CbAbsoluteRotate] updating min vel theta local planner to: %lf, from previous value: %lf ",
               -*maxVelTheta, this->oldMinVelTheta);
    }
  }
  else
  {
    if (yawGoalTolerance)
    {
      yaw_goal_tolerance.value = oldYawTolerance;
      conf.doubles.push_back(yaw_goal_tolerance);
      ROS_INFO("[CbAbsoluteRotate] restoring yaw tolerance local planner from: %lf, to previous value: %lf ",
               *yawGoalTolerance, this->oldYawTolerance);
    }
 
    if (maxVelTheta)
    {
      if (isRosBasePlanner)
      {
        max_vel_theta.value = oldMaxVelTheta;
        min_vel_theta.value = oldMinVelTheta;
      }
 
      conf.doubles.push_back(max_vel_theta);
      conf.doubles.push_back(min_vel_theta);
      ROS_INFO("[CbAbsoluteRotate] restoring max vel theta local planner from: %lf, to previous value: %lf ",
               *maxVelTheta, this->oldMaxVelTheta);
      ROS_INFO("[CbAbsoluteRotate] restoring min vel theta local planner from: %lf, to previous value: %lf ",
               -(*maxVelTheta), this->oldMinVelTheta);
    }
  }
 
  srv_req.config = conf;
  bool res;
  do
  {
    std::string servername = nodename + "/" + localPlannerName + "/set_parameters";
    res = ros::service::call(servername, srv_req, srv_resp);
    ROS_INFO_STREAM("[CbAbsoluteRotate] dynamic configure call [" << servername << "]: " << res);
    ros::spinOnce();
    if (!res)
    {
      ros::Duration(0.1).sleep();
      ROS_INFO("[CbAbsoluteRotate] Failed, retrtying call");
    }
  } while (!res);
}

References cl_move_base_z::Default, maxVelTheta, oldMaxVelTheta, oldMinVelTheta, oldYawTolerance, cl_move_base_z::PureSpinning, spinningPlanner, and yawGoalTolerance.

Referenced by onEntry(), and onExit().

Here is the caller graph for this function:

Member Data Documentation

absoluteGoalAngleDegree

boost::optional<float> cl_move_base_z::CbAbsoluteRotate::absoluteGoalAngleDegree

Definition at line 26 of file cb_absolute_rotate.h.

Referenced by CbAbsoluteRotate(), and onEntry().

listener

tf::TransformListener cl_move_base_z::CbAbsoluteRotate::listener

Definition at line 24 of file cb_absolute_rotate.h.

maxVelTheta

boost::optional<float> cl_move_base_z::CbAbsoluteRotate::maxVelTheta

Definition at line 28 of file cb_absolute_rotate.h.

Referenced by updateTemporalBehaviorParameters().

oldMaxVelTheta

float cl_move_base_z::CbAbsoluteRotate::oldMaxVelTheta

private

Definition at line 41 of file cb_absolute_rotate.h.

Referenced by updateTemporalBehaviorParameters().

oldMinVelTheta

float cl_move_base_z::CbAbsoluteRotate::oldMinVelTheta

private

Definition at line 42 of file cb_absolute_rotate.h.

Referenced by updateTemporalBehaviorParameters().

oldYawTolerance

float cl_move_base_z::CbAbsoluteRotate::oldYawTolerance

private

Definition at line 40 of file cb_absolute_rotate.h.

Referenced by updateTemporalBehaviorParameters().

spinningPlanner

boost::optional<SpiningPlanner> cl_move_base_z::CbAbsoluteRotate::spinningPlanner

Definition at line 29 of file cb_absolute_rotate.h.

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

yawGoalTolerance

boost::optional<float> cl_move_base_z::CbAbsoluteRotate::yawGoalTolerance

Definition at line 27 of file cb_absolute_rotate.h.

Referenced by CbAbsoluteRotate(), and updateTemporalBehaviorParameters().

---

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

• smacc_client_library/move_base_z_client/move_base_z_client_plugin/include/move_base_z_client_plugin/client_behaviors/cb_absolute_rotate.h
• smacc_client_library/move_base_z_client/move_base_z_client_plugin/src/client_behaviors/cb_absolute_rotate.cpp