cl_move_base_z::CbNavigateBackwards Class Reference

#include <cb_navigate_backwards.h>

Inheritance diagram for cl_move_base_z::CbNavigateBackwards:

Collaboration diagram for cl_move_base_z::CbNavigateBackwards:

Public Member Functions
	CbNavigateBackwards (float backwardDistance)
	CbNavigateBackwards ()
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
boost::optional< float >	backwardDistance
boost::optional< float >	backwardSpeed
tf::TransformListener	listener
cl_move_base_z::odom_tracker::OdomTracker *	odomTracker_

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 15 of file cb_navigate_backwards.h.

Constructor & Destructor Documentation

CbNavigateBackwards() [1/2]

cl_move_base_z::CbNavigateBackwards::CbNavigateBackwards

(

float

backwardDistance

)

Definition at line 10 of file cb_navigate_backward.cpp.

{
  if (backwardDistance < 0)
  {
    ROS_ERROR("cb backward: distance must be greater or equal than 0");
    this->backwardDistance = 0;
  }
  this->backwardDistance = backwardDistance;
}

References backwardDistance.

CbNavigateBackwards() [2/2]

cl_move_base_z::CbNavigateBackwards::CbNavigateBackwards

(

)

Definition at line 20 of file cb_navigate_backward.cpp.

{
}

Member Function Documentation

onEntry()

void cl_move_base_z::CbNavigateBackwards::onEntry ( )

overridevirtual

Reimplemented from smacc::ISmaccClientBehavior.

Definition at line 24 of file cb_navigate_backward.cpp.

{
  // straight motion distance
  double dist;
 
  if (!backwardDistance)
  {
    this->getCurrentState()->param("straight_motion_distance", dist, 3.5);
  }
  else
  {
    dist = *backwardDistance;
  }
 
  ROS_INFO_STREAM("[CbNavigateBackwards] Straight backwards motion distance: " << dist);
 
  auto p = moveBaseClient_->getComponent<cl_move_base_z::Pose>();
  auto referenceFrame = p->getReferenceFrame();
  auto currentPoseMsg = p->toPoseMsg();
 
  tf::Transform currentPose;
  tf::poseMsgToTF(currentPoseMsg, currentPose);
  tf::Transform forwardDeltaTransform;
  forwardDeltaTransform.setIdentity();
  forwardDeltaTransform.setOrigin(tf::Vector3(-dist, 0, 0));
  tf::Transform targetPose = currentPose * forwardDeltaTransform;
  ClMoveBaseZ::Goal goal;
  goal.target_pose.header.frame_id = referenceFrame;
  goal.target_pose.header.stamp = ros::Time::now();
  tf::poseTFToMsg(targetPose, goal.target_pose.pose);
  ROS_INFO_STREAM("[CbNavigateBackwards] TARGET POSE BACKWARDS: " << goal.target_pose);
 
  odomTracker_ = moveBaseClient_->getComponent<OdomTracker>();
  if (odomTracker_ != nullptr)
  {
    this->odomTracker_->clearPath();
    this->odomTracker_->setStartPoint(currentPoseMsg);
    this->odomTracker_->setWorkingMode(WorkingMode::RECORD_PATH);
  }
 
  auto plannerSwitcher = moveBaseClient_->getComponent<PlannerSwitcher>();
  plannerSwitcher->setBackwardPlanner();
  // this wait might be needed to let the backward planner receive the last forward message from the odom tracker
  ros::Duration(0.5).sleep();
 
  moveBaseClient_->sendGoal(goal);
}

References backwardDistance, cl_move_base_z::odom_tracker::OdomTracker::clearPath(), smacc::ISmaccClient::getComponent(), smacc::ISmaccClientBehavior::getCurrentState(), cl_move_base_z::Pose::getReferenceFrame(), cl_move_base_z::CbMoveBaseClientBehaviorBase::moveBaseClient_, odomTracker_, smacc::ISmaccState::param(), smacc::client_bases::SmaccActionClientBase< ActionType >::sendGoal(), cl_move_base_z::PlannerSwitcher::setBackwardPlanner(), cl_move_base_z::odom_tracker::OdomTracker::setStartPoint(), and cl_move_base_z::odom_tracker::OdomTracker::setWorkingMode().

Here is the call graph for this function:

onExit()

void cl_move_base_z::CbNavigateBackwards::onExit ( )

overridevirtual

Reimplemented from smacc::ISmaccClientBehavior.

Definition at line 72 of file cb_navigate_backward.cpp.

{
  if (odomTracker_)
  {
    this->odomTracker_->setWorkingMode(WorkingMode::IDLE);
  }
}

References odomTracker_, and cl_move_base_z::odom_tracker::OdomTracker::setWorkingMode().

Here is the call graph for this function:

Member Data Documentation

backwardDistance

boost::optional<float> cl_move_base_z::CbNavigateBackwards::backwardDistance

Definition at line 18 of file cb_navigate_backwards.h.

Referenced by CbNavigateBackwards(), and onEntry().

backwardSpeed

boost::optional<float> cl_move_base_z::CbNavigateBackwards::backwardSpeed

Definition at line 21 of file cb_navigate_backwards.h.

listener

tf::TransformListener cl_move_base_z::CbNavigateBackwards::listener

Definition at line 23 of file cb_navigate_backwards.h.

odomTracker_

cl_move_base_z::odom_tracker::OdomTracker* cl_move_base_z::CbNavigateBackwards::odomTracker_

Definition at line 25 of file cb_navigate_backwards.h.

Referenced by onEntry(), and onExit().

---

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_navigate_backwards.h
• smacc_client_library/move_base_z_client/move_base_z_client_plugin/src/client_behaviors/cb_navigate_backward.cpp