cl_move_base_z::CbNavigateForward Class Reference

#include <cb_navigate_forward.h>

Inheritance diagram for cl_move_base_z::CbNavigateForward:

Collaboration diagram for cl_move_base_z::CbNavigateForward:

Public Member Functions
	CbNavigateForward (float forwardDistance)
	CbNavigateForward ()
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 >	forwardDistance
boost::optional< float >	forwardSpeed
boost::optional< geometry_msgs::Quaternion >	forceInitialOrientation
tf::TransformListener	listener
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 14 of file cb_navigate_forward.h.

Constructor & Destructor Documentation

CbNavigateForward() [1/2]

cl_move_base_z::CbNavigateForward::CbNavigateForward

(

float

forwardDistance

)

Definition at line 8 of file cb_navigate_forward.cpp.

{
  this->forwardDistance = forwardDistance;
}

References forwardDistance.

CbNavigateForward() [2/2]

cl_move_base_z::CbNavigateForward::CbNavigateForward

(

)

Definition at line 13 of file cb_navigate_forward.cpp.

{
}

Member Function Documentation

onEntry()

void cl_move_base_z::CbNavigateForward::onEntry ( )

overridevirtual

Reimplemented from smacc::ISmaccClientBehavior.

Definition at line 17 of file cb_navigate_forward.cpp.

{
  // straight motion distance
  double dist;
 
  if (!forwardDistance)
  {
    this->getCurrentState()->param("straight_motion_distance", dist, 3.5);
  }
  else
  {
    dist = *forwardDistance;
  }
 
  ROS_INFO_STREAM("[CbNavigateForward] Straight motion distance: " << dist);
 
  auto p = moveBaseClient_->getComponent<cl_move_base_z::Pose>();
  auto referenceFrame = p->getReferenceFrame();
  auto currentPoseMsg = p->toPoseMsg();
 
  if (this->forceInitialOrientation)
  {
    currentPoseMsg.orientation = *forceInitialOrientation;
    ROS_WARN_STREAM("[CbNavigateForward] Forcing initial straight motion orientation: " << currentPoseMsg.orientation);
  }
 
  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("TARGET POSE FORWARD: " << goal.target_pose.pose);
 
  geometry_msgs::PoseStamped currentStampedPoseMsg;
  currentStampedPoseMsg.header.frame_id = referenceFrame;
  currentStampedPoseMsg.header.stamp = ros::Time::now();
  tf::poseTFToMsg(currentPose, currentStampedPoseMsg.pose);
 
  odomTracker_ = moveBaseClient_->getComponent<OdomTracker>();
  odomTracker_->pushPath("StraightNavigationForwards");
 
  odomTracker_->setStartPoint(currentStampedPoseMsg);
  odomTracker_->setWorkingMode(WorkingMode::RECORD_PATH);
 
  auto plannerSwitcher = moveBaseClient_->getComponent<PlannerSwitcher>();
  plannerSwitcher->setForwardPlanner();
 
  moveBaseClient_->sendGoal(goal);
}

References forceInitialOrientation, forwardDistance, smacc::ISmaccClient::getComponent(), smacc::ISmaccClientBehavior::getCurrentState(), cl_move_base_z::Pose::getReferenceFrame(), cl_move_base_z::CbMoveBaseClientBehaviorBase::moveBaseClient_, odomTracker_, smacc::ISmaccState::param(), cl_move_base_z::odom_tracker::OdomTracker::pushPath(), smacc::client_bases::SmaccActionClientBase< ActionType >::sendGoal(), cl_move_base_z::PlannerSwitcher::setForwardPlanner(), 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::CbNavigateForward::onExit ( )

overridevirtual

Reimplemented from smacc::ISmaccClientBehavior.

Definition at line 76 of file cb_navigate_forward.cpp.

{
  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

forceInitialOrientation

boost::optional<geometry_msgs::Quaternion> cl_move_base_z::CbNavigateForward::forceInitialOrientation

Definition at line 26 of file cb_navigate_forward.h.

Referenced by onEntry().

forwardDistance

boost::optional<float> cl_move_base_z::CbNavigateForward::forwardDistance

Definition at line 17 of file cb_navigate_forward.h.

Referenced by CbNavigateForward(), and onEntry().

forwardSpeed

boost::optional<float> cl_move_base_z::CbNavigateForward::forwardSpeed

Definition at line 20 of file cb_navigate_forward.h.

listener

tf::TransformListener cl_move_base_z::CbNavigateForward::listener

Definition at line 28 of file cb_navigate_forward.h.

odomTracker_

odom_tracker::OdomTracker* cl_move_base_z::CbNavigateForward::odomTracker_

Definition at line 30 of file cb_navigate_forward.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_forward.h
• smacc_client_library/move_base_z_client/move_base_z_client_plugin/src/client_behaviors/cb_navigate_forward.cpp