#include <cb_undo_path_backwards2.h>

Inheritance diagram for cl_move_base_z::CbUndoPathBackwards2:

Collaboration diagram for cl_move_base_z::CbUndoPathBackwards2:

Public Member Functions
	CbUndoPathBackwards2 ()

virtual void	onEntry () override

virtual void	onExit () override

virtual void	update () override

template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()

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 Member Functions inherited from smacc::ISmaccUpdatable
	ISmaccUpdatable ()

	ISmaccUpdatable (ros::Duration duration)

void	executeUpdate ()

void	setUpdatePeriod (ros::Duration duration)

Private Member Functions
void	publishMarkers ()

float	evalPlaneSide (const geometry_msgs::Pose &pose)

Private Attributes
ClMoveBaseZ::Goal	goal_

odom_tracker::OdomTracker *	odomTracker_

tf::TransformListener	listener

cl_move_base_z::Pose *	robotPose_

std::atomic< bool >	goalLinePassed_

float	initial_plane_side_

float	triggerThreshold_

std::function< void()>	postVirtualLinePassed_

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

virtual void	update ()=0

Protected Attributes inherited from cl_move_base_z::CbMoveBaseClientBehaviorBase
ClMoveBaseZ *	moveBaseClient_

ros::Publisher	visualizationMarkersPub_

Detailed Description

Definition at line 21 of file cb_undo_path_backwards2.h.

Constructor & Destructor Documentation

◆ CbUndoPathBackwards2()

cl_move_base_z::CbUndoPathBackwards2::CbUndoPathBackwards2 ( )

Definition at line 15 of file cb_undo_path_backwards2.cpp.

                                           : goalLinePassed_(false)
{
  triggerThreshold_ = 0.01;  // 10 mm
}

References triggerThreshold_.

Member Function Documentation

◆ evalPlaneSide()

float cl_move_base_z::CbUndoPathBackwards2::evalPlaneSide ( const geometry_msgs::Pose & pose )

private

Definition at line 48 of file cb_undo_path_backwards2.cpp.

{
  // check the line was passed
  // y = mx x + y0
  // y = (sin(alpha)/cos(alpha)) (x -x0) + y0
  // cos(alpha)(y - y0) - sin(alpha) (x -x0)= 0 // if greater one side if lower , the other
  auto y = pose.position.y;
  auto x = pose.position.x;
  auto alpha = tf::getYaw(goal_.target_pose.pose.orientation);
  auto y0 = goal_.target_pose.pose.position.y;
  auto x0 = goal_.target_pose.pose.position.x;
 
  auto evalimplicit = cos(alpha) * (y - y0) - sin (alpha) * (x - x0);
  return evalimplicit;
}

References goal_.

Referenced by onEntry(), and update().

Here is the caller graph for this function:

◆ onEntry()

void cl_move_base_z::CbUndoPathBackwards2::onEntry ( )

overridevirtual

Reimplemented from smacc::ISmaccClientBehavior.

Definition at line 20 of file cb_undo_path_backwards2.cpp.

{
  odomTracker_ = moveBaseClient_->getComponent<OdomTracker>();
 
  auto plannerSwitcher = moveBaseClient_->getComponent<PlannerSwitcher>();
  robotPose_ = moveBaseClient_->getComponent<cl_move_base_z::Pose>();
 
  nav_msgs::Path forwardpath = odomTracker_->getPath();
  // ROS_INFO_STREAM("[UndoPathBackward] Current path backwards: " << forwardpath);
 
  odomTracker_->setWorkingMode(WorkingMode::CLEAR_PATH);
 
  // this line is used to flush/reset backward planner in the case it were already there
  // plannerSwitcher->setDefaultPlanners();
  if (forwardpath.poses.size() > 0)
  {
    goal_.target_pose = forwardpath.poses.front();
    initial_plane_side_ = evalPlaneSide(robotPose_->toPoseMsg());
    plannerSwitcher->setUndoPathBackwardsPlannerConfiguration();
    moveBaseClient_->sendGoal(goal_);
  }
}

References evalPlaneSide(), smacc::ISmaccClient::getComponent(), cl_move_base_z::odom_tracker::OdomTracker::getPath(), goal_, initial_plane_side_, cl_move_base_z::CbMoveBaseClientBehaviorBase::moveBaseClient_, odomTracker_, robotPose_, smacc::client_bases::SmaccActionClientBase< ActionType >::sendGoal(), cl_move_base_z::odom_tracker::OdomTracker::setWorkingMode(), and cl_move_base_z::Pose::toPoseMsg().

Here is the call graph for this function:

◆ onExit()

void cl_move_base_z::CbUndoPathBackwards2::onExit ( )

overridevirtual

Reimplemented from smacc::ISmaccClientBehavior.

Definition at line 127 of file cb_undo_path_backwards2.cpp.

{
  auto* odomTracker = moveBaseClient_->getComponent<OdomTracker>();
  odomTracker->popPath();
}

References smacc::ISmaccClient::getComponent(), and cl_move_base_z::CbMoveBaseClientBehaviorBase::moveBaseClient_.

Here is the call graph for this function:

◆ onOrthogonalAllocation()

template<typename TOrthogonal , typename TSourceObject >

void cl_move_base_z::CbUndoPathBackwards2::onOrthogonalAllocation ( )

inline

Definition at line 32 of file cb_undo_path_backwards2.h.

  {
    CbMoveBaseClientBehaviorBase::onOrthogonalAllocation<TOrthogonal, TSourceObject>();
 
    postVirtualLinePassed_ = [=] { this->postEvent<EvGoalVirtualLinePassed<TSourceObject, TOrthogonal>>(); };
  }

References postVirtualLinePassed_.

◆ publishMarkers()

void cl_move_base_z::CbUndoPathBackwards2::publishMarkers ( )

private

Definition at line 90 of file cb_undo_path_backwards2.cpp.

{
  double phi = tf::getYaw(goal_.target_pose.pose.orientation);
  visualization_msgs::Marker marker;
  marker.header.frame_id = robotPose_->getReferenceFrame();
 
  marker.header.stamp = ros::Time::now();
  marker.ns = "my_namespace2";
  marker.id = 0;
  marker.type = visualization_msgs::Marker::SPHERE;
  marker.action = visualization_msgs::Marker::ADD;
  marker.scale.x = 3;
  marker.scale.y = 3;
  marker.scale.z = 3;
  marker.color.a = 1.0;
  marker.color.r = 1.0;
  marker.color.g = 0.0;
  marker.color.b = 0.0;
 
  marker.pose = goal_.target_pose.pose;
 
  // geometry_msgs::Point start, end;
  // start.x = pose.position.x;
  // start.y = pose.position.y;
 
  // end.x = pose.position.x + 0.5 * cos(phi);
  // end.y = pose.position.y + 0.5 * sin(phi);
 
  // marker.points.push_back(start);
  // marker.points.push_back(end);
 
  visualization_msgs::MarkerArray ma;
  ma.markers.push_back(marker);
 
  this->visualizationMarkersPub_.publish(ma);
}

References cl_move_base_z::Pose::getReferenceFrame(), goal_, robotPose_, and cl_move_base_z::CbMoveBaseClientBehaviorBase::visualizationMarkersPub_.

Here is the call graph for this function:

◆ update()

void cl_move_base_z::CbUndoPathBackwards2::update ( )

overridevirtual

Implements smacc::ISmaccUpdatable.

Definition at line 64 of file cb_undo_path_backwards2.cpp.

{
  auto pose = robotPose_->toPoseMsg();
 
  float evalimplicit = evalPlaneSide(pose);
  if (sgn(evalimplicit) != sgn(initial_plane_side_))
  {
    ROS_WARN_STREAM("[CbUndoPathBackwards2] goal_ line passed: "
                                      << evalimplicit << "/" << this->initial_plane_side_);
 
    if (fabs(evalimplicit) > triggerThreshold_ /*meters*/)
    {
       ROS_WARN_STREAM("[CbUndoPathBackwards2] virtual goal line passed, stopping behavior and success: "
                                      << evalimplicit << "/" << this->initial_plane_side_);
      moveBaseClient_->cancelGoal();
      // this->postSuccessEvent();
      this->postVirtualLinePassed_();
    }
  }
  else
  {
    ROS_INFO_STREAM_THROTTLE(1.0, "[CbUndoPathBackwards2] goal_ line not passed yet: " << evalimplicit << "/"
                                                                                       << this->initial_plane_side_);
  }
}

References smacc::client_bases::SmaccActionClientBase< ActionType >::cancelGoal(), evalPlaneSide(), initial_plane_side_, cl_move_base_z::CbMoveBaseClientBehaviorBase::moveBaseClient_, postVirtualLinePassed_, robotPose_, cl_move_base_z::sgn(), cl_move_base_z::Pose::toPoseMsg(), and triggerThreshold_.

Here is the call graph for this function:

Member Data Documentation

◆ goal_

ClMoveBaseZ::Goal cl_move_base_z::CbUndoPathBackwards2::goal_

private

Definition at line 43 of file cb_undo_path_backwards2.h.

Referenced by evalPlaneSide(), onEntry(), and publishMarkers().

◆ goalLinePassed_

std::atomic<bool> cl_move_base_z::CbUndoPathBackwards2::goalLinePassed_

private

Definition at line 48 of file cb_undo_path_backwards2.h.

◆ initial_plane_side_

float cl_move_base_z::CbUndoPathBackwards2::initial_plane_side_

private

Definition at line 49 of file cb_undo_path_backwards2.h.

Referenced by onEntry(), and update().

◆ listener

tf::TransformListener cl_move_base_z::CbUndoPathBackwards2::listener

private

Definition at line 46 of file cb_undo_path_backwards2.h.

◆ odomTracker_

odom_tracker::OdomTracker* cl_move_base_z::CbUndoPathBackwards2::odomTracker_

private

Definition at line 44 of file cb_undo_path_backwards2.h.

Referenced by onEntry().

◆ postVirtualLinePassed_

std::function<void()> cl_move_base_z::CbUndoPathBackwards2::postVirtualLinePassed_

private

Definition at line 51 of file cb_undo_path_backwards2.h.

Referenced by onOrthogonalAllocation(), and update().

◆ robotPose_

cl_move_base_z::Pose* cl_move_base_z::CbUndoPathBackwards2::robotPose_

private

Definition at line 47 of file cb_undo_path_backwards2.h.

Referenced by onEntry(), publishMarkers(), and update().

◆ triggerThreshold_

float cl_move_base_z::CbUndoPathBackwards2::triggerThreshold_

private

Definition at line 50 of file cb_undo_path_backwards2.h.

Referenced by CbUndoPathBackwards2(), and update().

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_undo_path_backwards2.h
smacc_client_library/move_base_z_client/move_base_z_client_plugin/src/client_behaviors/cb_undo_path_backwards2.cpp

Public Member Functions

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ CbUndoPathBackwards2()

Member Function Documentation

◆ evalPlaneSide()

◆ onEntry()

◆ onExit()

◆ onOrthogonalAllocation()

◆ publishMarkers()

◆ update()

Member Data Documentation

◆ goal_

◆ goalLinePassed_

◆ initial_plane_side_

◆ listener

◆ odomTracker_

◆ postVirtualLinePassed_

◆ robotPose_

◆ triggerThreshold_