#include <cp_odom_tracker.hpp>

Inheritance diagram for cl_nav2z::odom_tracker::CpOdomTracker:

Collaboration diagram for cl_nav2z::odom_tracker::CpOdomTracker:

Classes
struct	PathInfo

Public Member Functions
	CpOdomTracker (std::string odomtopicName="/odom", std::string odomFrame="odom")

virtual void	processOdometryMessage (const nav_msgs::msg::Odometry::SharedPtr odom)
	odom callback: Updates the path - this must be called periodically for each odometry message.

void	setWorkingMode (WorkingMode workingMode)

void	setPublishMessages (bool value)

void	pushPath ()

void	pushPath (std::string pathname)

void	popPath (int pathCount=1, bool keepPreviousPath=false)

void	clearPath ()

void	setStartPoint (const geometry_msgs::msg::PoseStamped &pose)

void	setStartPoint (const geometry_msgs::msg::Pose &pose)

void	setCurrentMotionGoal (const geometry_msgs::msg::PoseStamped &pose)

void	setCurrentPathName (const std::string &currentPathName)

std::optional< geometry_msgs::msg::PoseStamped >	getCurrentMotionGoal ()

nav_msgs::msg::Path	getPath ()

void	logStateString (bool debug=true)

Public Member Functions inherited from smacc2::ISmaccComponent
	ISmaccComponent ()

virtual	~ISmaccComponent ()

virtual std::string	getName () const

Protected Member Functions
void	onInitialize () override

void	updateConfiguration ()

virtual void	rtPublishPaths (rclcpp::Time timestamp)

virtual bool	updateRecordPath (const nav_msgs::msg::Odometry &odom)

virtual bool	updateClearPath (const nav_msgs::msg::Odometry &odom)

void	updateAggregatedStackPath ()

Protected Member Functions inherited from smacc2::ISmaccComponent
template<typename EventType >
void	postEvent (const EventType &ev)

template<typename EventType >
void	postEvent ()

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

template<typename TComponent >
void	requiresComponent (TComponent *&requiredComponentStorage, bool throwExceptionIfNotExist=false)

template<typename TComponent >
void	requiresComponent (std::string name, TComponent *&requiredComponentStorage, bool throwExceptionIfNotExist=false)

template<typename TClient >
void	requiresClient (TClient *&requiredClientStorage)

template<typename SmaccComponentType , typename TOrthogonal , typename TClient , typename... TArgs>
SmaccComponentType *	createSiblingComponent (TArgs... targs)

template<typename SmaccComponentType , typename TOrthogonal , typename TClient , typename... TArgs>
SmaccComponentType *	createSiblingNamedComponent (std::string name, TArgs... targs)

rclcpp::Node::SharedPtr	getNode ()

rclcpp::Logger	getLogger () const

ISmaccStateMachine *	getStateMachine ()

Protected Attributes
rclcpp::Publisher< nav_msgs::msg::Path >::SharedPtr	robotBasePathPub_

rclcpp::Publisher< nav_msgs::msg::Path >::SharedPtr	robotBasePathStackedPub_

rclcpp::Subscription< nav_msgs::msg::Odometry >::SharedPtr	odomSub_

double	recordPointDistanceThreshold_
	How much distance there is between two points of the path.

double	recordAngularDistanceThreshold_
	Meters.

double	clearPointDistanceThreshold_
	rads

double	clearAngularDistanceThreshold_
	rads

std::string	odomFrame_

std::string	odomTopicName_

bool	publishMessages

nav_msgs::msg::Path	baseTrajectory_
	Processed path for the mouth of the reel.

WorkingMode	workingMode_

std::vector< nav_msgs::msg::Path >	pathStack_

std::vector< PathInfo >	pathInfos_

nav_msgs::msg::Path	aggregatedStackPathMsg_

bool	subscribeToOdometryTopic_

std::optional< geometry_msgs::msg::PoseStamped >	currentMotionGoal_

std::string	currentPathName_

std::mutex	m_mutex_

Protected Attributes inherited from smacc2::ISmaccComponent
ISmaccStateMachine *	stateMachine_

ISmaccClient *	owner_

Detailed Description

This object tracks and saves the trajectories performed by the vehicle so that they can be used later to execute operations such as "undo motions"

Definition at line 53 of file cp_odom_tracker.hpp.

Constructor & Destructor Documentation

◆ CpOdomTracker()

cl_nav2z::odom_tracker::CpOdomTracker::CpOdomTracker	(	std::string	odomtopicName = `"/odom"`,
		std::string	odomFrame = `"odom"`
	)

Definition at line 32 of file cp_odom_tracker.cpp.

{
  workingMode_ = WorkingMode::RECORD_PATH;
  publishMessages = true;
  subscribeToOdometryTopic_ = true;
  odomFrame_ = odomFrame;
  odomTopicName_ = odomTopicName;
}

References odomFrame_, odomTopicName_, publishMessages, cl_nav2z::odom_tracker::RECORD_PATH, subscribeToOdometryTopic_, and workingMode_.

Member Function Documentation

◆ clearPath()

void cl_nav2z::odom_tracker::CpOdomTracker::clearPath ( )

Definition at line 279 of file cp_odom_tracker.cpp.

{
  std::lock_guard<std::mutex> lock(m_mutex_);
  baseTrajectory_.poses.clear();
 
  rtPublishPaths(getNode()->now());
  this->logStateString();
  this->updateAggregatedStackPath();
 
  this->currentMotionGoal_.reset();
}

References baseTrajectory_, currentMotionGoal_, smacc2::ISmaccComponent::getNode(), logStateString(), m_mutex_, rtPublishPaths(), and updateAggregatedStackPath().

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

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getCurrentMotionGoal()

std::optional< geometry_msgs::msg::PoseStamped > cl_nav2z::odom_tracker::CpOdomTracker::getCurrentMotionGoal ( )

Definition at line 337 of file cp_odom_tracker.cpp.

{
  std::lock_guard<std::mutex> lock(m_mutex_);
  return this->currentMotionGoal_;
}

References currentMotionGoal_, and m_mutex_.

Referenced by cl_nav2z::CbRetry< TCbRelativeMotion >::onEntry().

Here is the caller graph for this function:

◆ getPath()

nav_msgs::msg::Path cl_nav2z::odom_tracker::CpOdomTracker::getPath ( )

Definition at line 343 of file cp_odom_tracker.cpp.

{
  std::lock_guard<std::mutex> lock(m_mutex_);
  return this->baseTrajectory_;
}

References baseTrajectory_, and m_mutex_.

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

Here is the caller graph for this function:

◆ logStateString()

void cl_nav2z::odom_tracker::CpOdomTracker::logStateString ( bool debug = true )

Definition at line 237 of file cp_odom_tracker.cpp.

{
  std::stringstream ss;
  ss << "--- odom tracker state ---" << std::endl;
  ss
    << " - path stack -" << currentPathName_ << " -  size:"
    << pathStack_.size()
    //<< " goal: " << (getCurrentMotionGoal()? getCurrentMotionGoal()->pose: geometry_msgs::msg::Pose())
    << std::endl;
  ss << " - [STACK-HEAD active path size: " << baseTrajectory_.poses.size() << "]" << std::endl;
  int i = 0;
  for (auto & p : pathStack_ | boost::adaptors::reversed)
  {
    auto & pathinfo = pathInfos_[pathInfos_.size() - i - 1];
    std::string goalstr = "[]";
    if (pathinfo.goalPose)
    {
      std::stringstream ss;
      ss << *(pathinfo.goalPose);
      goalstr = ss.str();
    }
 
    ss << " - p " << i << "[" << p.header.stamp << "], size: " << p.poses.size() << " - "
       << pathinfo.name << " - goal: " << goalstr << std::endl;
    i++;
  }
  ss << "---";
 
  if (this->odomSub_->get_publisher_count() == 0)
  {
    ss << std::endl
       << "[CpOdomTracker] Odom tracker cannot record the path because there is no publisher to "
          "the odometry topic"
       << std::endl;
  }
 
  if (debug)
    RCLCPP_DEBUG(getLogger(), ss.str().c_str());
  else
    RCLCPP_INFO(getLogger(), ss.str().c_str());
}

References baseTrajectory_, currentPathName_, smacc2::ISmaccComponent::getLogger(), odomSub_, pathInfos_, and pathStack_.

Referenced by clearPath(), cl_nav2z::CbUndoPathBackwards::onEntry(), cl_nav2z::CbUndoPathBackwards::onExit(), popPath(), and pushPath().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ onInitialize()

void cl_nav2z::odom_tracker::CpOdomTracker::onInitialize ( )

overrideprotectedvirtual

Reimplemented from smacc2::ISmaccComponent.

Definition at line 57 of file cp_odom_tracker.cpp.

{
  // default values
  recordPointDistanceThreshold_ = 0.005;  // 5 mm
  recordAngularDistanceThreshold_ = 0.1;  // radians
  clearPointDistanceThreshold_ = 0.05;    // 5 cm
  clearAngularDistanceThreshold_ = 0.1;   // radi
 
  auto nh = getNode();
  RCLCPP_WARN(getLogger(), "[CpOdomTracker] Initializing Odometry Tracker");
 
  parameterDeclareAndtryGetOrSet(nh, "odom_frame", this->odomFrame_);
  parameterDeclareAndtryGetOrSet(
    nh, "record_point_distance_threshold", recordPointDistanceThreshold_);
  parameterDeclareAndtryGetOrSet(
    nh, "record_angular_distance_threshold", recordAngularDistanceThreshold_);
  parameterDeclareAndtryGetOrSet(
    nh, "clear_point_distance_threshold", clearPointDistanceThreshold_);
  parameterDeclareAndtryGetOrSet(
    nh, "clear_angular_distance_threshold", clearAngularDistanceThreshold_);
 
  if (this->subscribeToOdometryTopic_)
  {
    RCLCPP_INFO_STREAM(
      nh->get_logger(), "[CpOdomTracker] subscribing to odom topic: " << odomTopicName_);
 
    rclcpp::SensorDataQoS qos;
    odomSub_ = nh->create_subscription<nav_msgs::msg::Odometry>(
      odomTopicName_, qos,
      std::bind(&CpOdomTracker::processOdometryMessage, this, std::placeholders::_1));
  }
 
  robotBasePathPub_ =
    nh->create_publisher<nav_msgs::msg::Path>("odom_tracker_path", rclcpp::QoS(1));
  robotBasePathStackedPub_ =
    nh->create_publisher<nav_msgs::msg::Path>("odom_tracker_stacked_path", rclcpp::QoS(1));
}

References clearAngularDistanceThreshold_, clearPointDistanceThreshold_, smacc2::ISmaccComponent::getLogger(), smacc2::ISmaccComponent::getNode(), odomFrame_, odomSub_, odomTopicName_, cl_nav2z::odom_tracker::parameterDeclareAndtryGetOrSet(), processOdometryMessage(), recordAngularDistanceThreshold_, recordPointDistanceThreshold_, robotBasePathPub_, robotBasePathStackedPub_, and subscribeToOdometryTopic_.

Here is the call graph for this function:

◆ popPath()

void cl_nav2z::odom_tracker::CpOdomTracker::popPath	(	int	pathCount = `1`,
		bool	keepPreviousPath = `false`
	)

Definition at line 192 of file cp_odom_tracker.cpp.

{
  RCLCPP_INFO(getLogger(), "odom_tracker m_mutex acquire");
  std::lock_guard<std::mutex> lock(m_mutex_);
 
  RCLCPP_INFO(getLogger(), "POP PATH ENTRY");
  this->logStateString();
 
  if (this->odomSub_->get_publisher_count() == 0)
  {
    RCLCPP_ERROR_STREAM(
      getLogger(),
      "[CpOdomTracker] Odom tracker cannot record the path because there is no publisher to the "
      "odometry topic");
  }
 
  if (!keepPreviousPath)
  {
    baseTrajectory_.poses.clear();
  }
 
  while (popCount > 0 && !pathStack_.empty())
  {
    auto & stackedPath = pathStack_.back().poses;
    auto & stackedPathInfo = pathInfos_.back();
 
    baseTrajectory_.poses.insert(
      baseTrajectory_.poses.begin(), stackedPath.begin(), stackedPath.end());
    this->currentMotionGoal_ = stackedPathInfo.goalPose;
    pathStack_.pop_back();
    pathInfos_.pop_back();
    popCount--;
 
    RCLCPP_INFO(getLogger(), "POP PATH Iteration ");
    this->logStateString();
  }
 
  RCLCPP_INFO(getLogger(), "POP PATH EXITING");
  this->logStateString();
  RCLCPP_INFO(getLogger(), "odom_tracker m_mutex release");
  this->updateAggregatedStackPath();
 
  this->currentMotionGoal_.reset();
}

References baseTrajectory_, currentMotionGoal_, smacc2::ISmaccComponent::getLogger(), logStateString(), m_mutex_, odomSub_, pathInfos_, pathStack_, and updateAggregatedStackPath().

Referenced by CpOdomTrackerActionServer::execute(), and cl_nav2z::CbUndoPathBackwards::onExit().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ processOdometryMessage()

void cl_nav2z::odom_tracker::CpOdomTracker::processOdometryMessage ( const nav_msgs::msg::Odometry::SharedPtr odom )

virtual

odom callback: Updates the path - this must be called periodically for each odometry message.

processOdometryMessage()

Definition at line 547 of file cp_odom_tracker.cpp.

{
  RCLCPP_INFO_THROTTLE(
    getLogger(), *getNode()->get_clock(), 5000,
    "[odom_tracker] processing odom msg update heartbeat");
  std::lock_guard<std::mutex> lock(m_mutex_);
 
  updateConfiguration();
 
  if (workingMode_ == WorkingMode::RECORD_PATH)
  {
    updateRecordPath(*odom);
  }
  else if (workingMode_ == WorkingMode::CLEAR_PATH)
  {
    updateClearPath(*odom);
  }
 
  // RCLCPP_WARN(getLogger(),"odomTracker odometry callback");
  if (publishMessages)
  {
    rtPublishPaths(odom->header.stamp);
  }
 
  // RCLCPP_INFO(getLogger(),"odom_tracker m_mutex release");
}

References cl_nav2z::odom_tracker::CLEAR_PATH, smacc2::ISmaccComponent::getLogger(), smacc2::ISmaccComponent::getNode(), m_mutex_, publishMessages, cl_nav2z::odom_tracker::RECORD_PATH, rtPublishPaths(), updateClearPath(), updateConfiguration(), updateRecordPath(), and workingMode_.

Referenced by onInitialize().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ pushPath() [1/2]

void cl_nav2z::odom_tracker::CpOdomTracker::pushPath ( )

Definition at line 154 of file cp_odom_tracker.cpp.

154{ this->pushPath(std::string()); }

cl_nav2z::odom_tracker::CpOdomTracker::pushPath

void pushPath()

Definition cp_odom_tracker.cpp:154

References pushPath().

Referenced by CpOdomTrackerActionServer::execute(), cl_nav2z::CbNavigateForward::onEntry(), and pushPath().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ pushPath() [2/2]

void cl_nav2z::odom_tracker::CpOdomTracker::pushPath ( std::string pathname )

Definition at line 156 of file cp_odom_tracker.cpp.

{
  RCLCPP_INFO(getLogger(), "odom_tracker m_mutex acquire");
  std::lock_guard<std::mutex> lock(m_mutex_);
 
  this->logStateString(false);
  if (this->odomSub_->get_publisher_count() == 0)
  {
    RCLCPP_ERROR_STREAM(
      getLogger(),
      "[CpOdomTracker] Odom tracker cannot record the path because there is no publisher to the "
      "odometry topic");
  }
 
  pathInfos_.push_back({currentPathName_, this->currentMotionGoal_});
  pathStack_.push_back(baseTrajectory_);
 
  geometry_msgs::msg::PoseStamped goalPose;
  if (this->currentMotionGoal_) goalPose = *this->currentMotionGoal_;
 
  RCLCPP_INFO_STREAM(
    getLogger(), "[CpOdomTracker] currentPathName: " << pathname
                                                     << "  size: " << baseTrajectory_.poses.size()
                                                     << "  current motion goal: " << goalPose);
 
  currentPathName_ = pathname;
 
  // clean the current trajectory to start a new one
  baseTrajectory_.poses.clear();
 
  this->logStateString(false);
  this->updateAggregatedStackPath();
 
  this->currentMotionGoal_.reset();
}

References baseTrajectory_, currentMotionGoal_, currentPathName_, smacc2::ISmaccComponent::getLogger(), logStateString(), m_mutex_, odomSub_, pathInfos_, pathStack_, and updateAggregatedStackPath().

Here is the call graph for this function:

◆ rtPublishPaths()

void cl_nav2z::odom_tracker::CpOdomTracker::rtPublishPaths ( rclcpp::Time timestamp )

protectedvirtual

rtPublishPaths()

Definition at line 354 of file cp_odom_tracker.cpp.

{
  baseTrajectory_.header.stamp = timestamp;
  robotBasePathPub_->publish(baseTrajectory_);
 
  aggregatedStackPathMsg_.header.stamp = timestamp;
  robotBasePathStackedPub_->publish(aggregatedStackPathMsg_);
}

References aggregatedStackPathMsg_, baseTrajectory_, robotBasePathPub_, and robotBasePathStackedPub_.

Referenced by clearPath(), and processOdometryMessage().

Here is the caller graph for this function:

◆ setCurrentMotionGoal()

void cl_nav2z::odom_tracker::CpOdomTracker::setCurrentMotionGoal ( const geometry_msgs::msg::PoseStamped & pose )

Definition at line 331 of file cp_odom_tracker.cpp.

{
  std::lock_guard<std::mutex> lock(m_mutex_);
  this->currentMotionGoal_ = pose;
}

References currentMotionGoal_, and m_mutex_.

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

Here is the caller graph for this function:

◆ setCurrentPathName()

void cl_nav2z::odom_tracker::CpOdomTracker::setCurrentPathName ( const std::string & currentPathName )

Definition at line 291 of file cp_odom_tracker.cpp.

{
  currentPathName_ = currentPathName;
}

References currentPathName_.

◆ setPublishMessages()

void cl_nav2z::odom_tracker::CpOdomTracker::setPublishMessages ( bool value )

setPublishMessages()

Definition at line 145 of file cp_odom_tracker.cpp.

{
  // RCLCPP_INFO(getLogger(),"odom_tracker m_mutex acquire");
  std::lock_guard<std::mutex> lock(m_mutex_);
  publishMessages = value;
  // RCLCPP_INFO(getLogger(),"odom_tracker m_mutex release");
  this->updateAggregatedStackPath();
}

References m_mutex_, publishMessages, and updateAggregatedStackPath().

Referenced by CpOdomTrackerActionServer::execute().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setStartPoint() [1/2]

void cl_nav2z::odom_tracker::CpOdomTracker::setStartPoint ( const geometry_msgs::msg::Pose & pose )

Definition at line 311 of file cp_odom_tracker.cpp.

{
  std::lock_guard<std::mutex> lock(m_mutex_);
  RCLCPP_INFO_STREAM(getLogger(), "[CpOdomTracker] set current path starting point: " << pose);
  geometry_msgs::msg::PoseStamped posestamped;
  posestamped.header.frame_id = this->odomFrame_;
  posestamped.header.stamp = getNode()->now();
  posestamped.pose = pose;
 
  if (baseTrajectory_.poses.size() > 0)
  {
    baseTrajectory_.poses[0] = posestamped;
  }
  else
  {
    baseTrajectory_.poses.push_back(posestamped);
  }
  this->updateAggregatedStackPath();
}

References baseTrajectory_, smacc2::ISmaccComponent::getLogger(), smacc2::ISmaccComponent::getNode(), m_mutex_, odomFrame_, and updateAggregatedStackPath().

Here is the call graph for this function:

◆ setStartPoint() [2/2]

void cl_nav2z::odom_tracker::CpOdomTracker::setStartPoint ( const geometry_msgs::msg::PoseStamped & pose )

Definition at line 296 of file cp_odom_tracker.cpp.

{
  std::lock_guard<std::mutex> lock(m_mutex_);
  RCLCPP_INFO_STREAM(getLogger(), "[CpOdomTracker] set current path starting point: " << pose);
  if (baseTrajectory_.poses.size() > 0)
  {
    baseTrajectory_.poses[0] = pose;
  }
  else
  {
    baseTrajectory_.poses.push_back(pose);
  }
  this->updateAggregatedStackPath();
}

References baseTrajectory_, smacc2::ISmaccComponent::getLogger(), m_mutex_, and updateAggregatedStackPath().

Referenced by cl_nav2z::CbNavigateBackwards::onEntry(), and cl_nav2z::CbNavigateForward::onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setWorkingMode()

void cl_nav2z::odom_tracker::CpOdomTracker::setWorkingMode ( WorkingMode workingMode )

setWorkingMode()

Definition at line 100 of file cp_odom_tracker.cpp.

{
  std::lock_guard<std::mutex> lock(m_mutex_);
 
  switch (workingMode)
  {
    case WorkingMode::RECORD_PATH:
      RCLCPP_INFO_STREAM(
        getLogger(),
        "[CpOdomTracker] setting working mode to RECORD - record_point_distance_threshold: "
          << recordPointDistanceThreshold_
          << ", record_angular_distance_threshold: " << recordAngularDistanceThreshold_);
      break;
    case WorkingMode::CLEAR_PATH:
      RCLCPP_INFO_STREAM(
        getLogger(),
        "[CpOdomTracker] setting working mode to CLEAR - clear_point_distance_threshold: "
          << clearPointDistanceThreshold_
          << ", clear_angular_distance_threshold: " << clearAngularDistanceThreshold_);
      break;
    case WorkingMode::IDLE:
      RCLCPP_INFO_STREAM(getLogger(), "[CpOdomTracker] setting working mode to IDLE");
      break;
    default:
 
      RCLCPP_INFO_STREAM(getLogger(), "[CpOdomTracker] setting working mode to <UNKNOWN>");
  }
 
  if (this->odomSub_->get_publisher_count() == 0)
  {
    RCLCPP_ERROR_STREAM(
      getLogger(),
      "[CpOdomTracker] Odom tracker cannot record the path because there is no publisher to the "
      "odometry topic");
  }
 
  workingMode_ = workingMode;
  // RCLCPP_INFO(getLogger(),"odom_tracker m_mutex release");
}

References cl_nav2z::odom_tracker::CLEAR_PATH, clearAngularDistanceThreshold_, clearPointDistanceThreshold_, smacc2::ISmaccComponent::getLogger(), cl_nav2z::odom_tracker::IDLE, m_mutex_, odomSub_, cl_nav2z::odom_tracker::RECORD_PATH, recordAngularDistanceThreshold_, recordPointDistanceThreshold_, and workingMode_.

Referenced by CpOdomTrackerActionServer::execute(), cl_nav2z::CbNavigateBackwards::onEntry(), cl_nav2z::CbNavigateForward::onEntry(), cl_nav2z::CbUndoPathBackwards::onEntry(), cl_nav2z::CbNavigateBackwards::onExit(), and cl_nav2z::CbNavigateForward::onExit().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ updateAggregatedStackPath()

void cl_nav2z::odom_tracker::CpOdomTracker::updateAggregatedStackPath ( )

protected

Definition at line 363 of file cp_odom_tracker.cpp.

{
  aggregatedStackPathMsg_.poses.clear();
  for (auto & p : pathStack_)
  {
    aggregatedStackPathMsg_.poses.insert(
      aggregatedStackPathMsg_.poses.end(), p.poses.begin(), p.poses.end());
  }
 
  aggregatedStackPathMsg_.header.frame_id = this->odomFrame_;
}

References aggregatedStackPathMsg_, odomFrame_, and pathStack_.

Referenced by clearPath(), popPath(), pushPath(), setPublishMessages(), setStartPoint(), and setStartPoint().

Here is the caller graph for this function:

◆ updateClearPath()

bool cl_nav2z::odom_tracker::CpOdomTracker::updateClearPath ( const nav_msgs::msg::Odometry & odom )

protectedvirtual

updateBackward()

Track robot base pose

Not removing point because it is enough far from the last cord point

Definition at line 380 of file cp_odom_tracker.cpp.

{
  // we initially accept any message if the queue is empty
  geometry_msgs::msg::PoseStamped base_pose;
 
  base_pose.pose = odom.pose.pose;
  base_pose.header = odom.header;
  baseTrajectory_.header = odom.header;
 
  bool acceptBackward = false;
  bool clearingError = false;
  bool finished = false;
 
  while (!finished)
  {
    if (
      baseTrajectory_.poses.size() <=
      1)  // we at least keep always the first point of the forward path when clearing
          // (this is important for backwards planner replanning and not losing the
          // last goal)
    {
      acceptBackward = false;
      finished = true;
    }
    else
    {
      auto & carrotPose = baseTrajectory_.poses.back().pose;
      auto & carrotPoint = carrotPose.position;
 
      double carrotAngle = tf2::getYaw(carrotPose.orientation);
 
      auto & currePose = base_pose.pose;
      auto & currePoint = currePose.position;
      double currentAngle = tf2::getYaw(currePose.orientation);
 
      double lastpointdist = p2pDistance(carrotPoint, currePoint);
      double goalAngleOffset = fabs(angles::shortest_angular_distance(carrotAngle, currentAngle));
 
      acceptBackward = !baseTrajectory_.poses.empty() &&
                       lastpointdist < clearPointDistanceThreshold_ &&
                       goalAngleOffset < clearAngularDistanceThreshold_;
 
      clearingError = lastpointdist > 2 * clearPointDistanceThreshold_;
      RCLCPP_DEBUG_STREAM(
        getLogger(), "[CpOdomTracker] clearing (accepted: " << acceptBackward
                                                            << ") linerr: " << lastpointdist
                                                            << ", anglerr: " << goalAngleOffset);
    }
 
    // RCLCPP_INFO(getLogger(),"Backwards, last distance: %lf < %lf accept: %d", dist,
    // minPointDistanceBackwardThresh_, acceptBackward);
    if (
      acceptBackward &&
      baseTrajectory_.poses.size() > 1) /* we always leave at least one item, specially interesting
                                                                                   for the backward local planner reach
                                                               the backwards goal with enough precision*/
    {
      baseTrajectory_.poses.pop_back();
    }
    else if (clearingError)
    {
      finished = true;
      RCLCPP_WARN(getLogger(), "[CpOdomTracker] Incorrect odom clearing motion.");
    }
    else
    {
      finished = true;
    }
  }
 
  return acceptBackward;
}

References baseTrajectory_, clearAngularDistanceThreshold_, clearPointDistanceThreshold_, smacc2::ISmaccComponent::getLogger(), and cl_nav2z::odom_tracker::p2pDistance().

Referenced by processOdometryMessage().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ updateConfiguration()

void cl_nav2z::odom_tracker::CpOdomTracker::updateConfiguration ( )

protected

reconfigCB()

Definition at line 518 of file cp_odom_tracker.cpp.

{
  if (!getNode()->get_parameter("odom_frame", this->odomFrame_))
  {
  }
 
  if (!getNode()->get_parameter("record_point_distance_threshold", recordPointDistanceThreshold_))
  {
  }
 
  if (!getNode()->get_parameter(
        "record_angular_distance_threshold", recordAngularDistanceThreshold_))
  {
  }
 
  if (!getNode()->get_parameter("clear_point_distance_threshold", clearPointDistanceThreshold_))
  {
  }
 
  if (!getNode()->get_parameter("clear_angular_distance_threshold", clearAngularDistanceThreshold_))
  {
  }
}

References clearAngularDistanceThreshold_, clearPointDistanceThreshold_, smacc2::ISmaccComponent::getNode(), odomFrame_, recordAngularDistanceThreshold_, and recordPointDistanceThreshold_.

Referenced by processOdometryMessage().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ updateRecordPath()

bool cl_nav2z::odom_tracker::CpOdomTracker::updateRecordPath ( const nav_msgs::msg::Odometry & odom )

protectedvirtual

updateRecordPath()

Track robot base pose

Definition at line 459 of file cp_odom_tracker.cpp.

{
  geometry_msgs::msg::PoseStamped base_pose;
 
  base_pose.pose = odom.pose.pose;
  base_pose.header = odom.header;
  baseTrajectory_.header = odom.header;
 
  bool enqueueOdomMessage = false;
 
  double dist = -1;
  if (baseTrajectory_.poses.empty())
  {
    enqueueOdomMessage = true;
  }
  else
  {
    const auto & prevPose = baseTrajectory_.poses.back().pose;
    const geometry_msgs::msg::Point & prevPoint = prevPose.position;
    double prevAngle = tf2::getYaw(prevPose.orientation);
 
    const geometry_msgs::msg::Point & currePoint = base_pose.pose.position;
    double currentAngle = tf2::getYaw(base_pose.pose.orientation);
 
    dist = p2pDistance(prevPoint, currePoint);
    double goalAngleOffset = fabs(angles::shortest_angular_distance(prevAngle, currentAngle));
 
    // RCLCPP_WARN(getLogger(),"dist %lf vs min %lf", dist, recordPointDistanceThreshold_);
 
    RCLCPP_WARN_THROTTLE(getLogger(), *getNode()->get_clock(), 2000, "odom received");
 
    if (dist > recordPointDistanceThreshold_ || goalAngleOffset > recordAngularDistanceThreshold_)
    {
      enqueueOdomMessage = true;
    }
    else
    {
      // RCLCPP_WARN(getLogger(),"skip odom, dist: %lf", dist);
      enqueueOdomMessage = false;
    }
  }
 
  if (enqueueOdomMessage)
  {
    RCLCPP_WARN_THROTTLE(
      getLogger(), *getNode()->get_clock(), 2000, "enqueue odom tracker pose. dist %lf vs min %lf",
      dist, recordPointDistanceThreshold_);
    baseTrajectory_.poses.push_back(base_pose);
  }
 
  return enqueueOdomMessage;
}

References baseTrajectory_, smacc2::ISmaccComponent::getLogger(), smacc2::ISmaccComponent::getNode(), cl_nav2z::odom_tracker::p2pDistance(), recordAngularDistanceThreshold_, and recordPointDistanceThreshold_.

Referenced by processOdometryMessage().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ aggregatedStackPathMsg_

nav_msgs::msg::Path cl_nav2z::odom_tracker::CpOdomTracker::aggregatedStackPathMsg_

protected

Definition at line 163 of file cp_odom_tracker.hpp.

Referenced by rtPublishPaths(), and updateAggregatedStackPath().

◆ baseTrajectory_

nav_msgs::msg::Path cl_nav2z::odom_tracker::CpOdomTracker::baseTrajectory_

protected

Processed path for the mouth of the reel.

Definition at line 149 of file cp_odom_tracker.hpp.

Referenced by clearPath(), getPath(), logStateString(), popPath(), pushPath(), rtPublishPaths(), setStartPoint(), setStartPoint(), updateClearPath(), and updateRecordPath().

◆ clearAngularDistanceThreshold_

double cl_nav2z::odom_tracker::CpOdomTracker::clearAngularDistanceThreshold_

protected

rads

Definition at line 138 of file cp_odom_tracker.hpp.

Referenced by onInitialize(), setWorkingMode(), updateClearPath(), and updateConfiguration().

◆ clearPointDistanceThreshold_

double cl_nav2z::odom_tracker::CpOdomTracker::clearPointDistanceThreshold_

protected

rads

Definition at line 135 of file cp_odom_tracker.hpp.

Referenced by onInitialize(), setWorkingMode(), updateClearPath(), and updateConfiguration().

◆ currentMotionGoal_

std::optional<geometry_msgs::msg::PoseStamped> cl_nav2z::odom_tracker::CpOdomTracker::currentMotionGoal_

protected

Definition at line 168 of file cp_odom_tracker.hpp.

Referenced by clearPath(), getCurrentMotionGoal(), popPath(), pushPath(), and setCurrentMotionGoal().

◆ currentPathName_

std::string cl_nav2z::odom_tracker::CpOdomTracker::currentPathName_

protected

Definition at line 169 of file cp_odom_tracker.hpp.

Referenced by logStateString(), pushPath(), and setCurrentPathName().

◆ m_mutex_

std::mutex cl_nav2z::odom_tracker::CpOdomTracker::m_mutex_

protected

Definition at line 171 of file cp_odom_tracker.hpp.

Referenced by clearPath(), getCurrentMotionGoal(), getPath(), popPath(), processOdometryMessage(), pushPath(), setCurrentMotionGoal(), setPublishMessages(), setStartPoint(), setStartPoint(), and setWorkingMode().

◆ odomFrame_

std::string cl_nav2z::odom_tracker::CpOdomTracker::odomFrame_

protected

Definition at line 140 of file cp_odom_tracker.hpp.

Referenced by CpOdomTracker(), onInitialize(), setStartPoint(), updateAggregatedStackPath(), and updateConfiguration().

◆ odomSub_

rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr cl_nav2z::odom_tracker::CpOdomTracker::odomSub_

protected

Definition at line 125 of file cp_odom_tracker.hpp.

Referenced by logStateString(), onInitialize(), popPath(), pushPath(), and setWorkingMode().

◆ odomTopicName_

std::string cl_nav2z::odom_tracker::CpOdomTracker::odomTopicName_

protected

Definition at line 142 of file cp_odom_tracker.hpp.

Referenced by CpOdomTracker(), and onInitialize().

◆ pathInfos_

std::vector<PathInfo> cl_nav2z::odom_tracker::CpOdomTracker::pathInfos_

protected

Definition at line 161 of file cp_odom_tracker.hpp.

Referenced by logStateString(), popPath(), and pushPath().

◆ pathStack_

std::vector<nav_msgs::msg::Path> cl_nav2z::odom_tracker::CpOdomTracker::pathStack_

protected

Definition at line 153 of file cp_odom_tracker.hpp.

Referenced by logStateString(), popPath(), pushPath(), and updateAggregatedStackPath().

◆ publishMessages

bool cl_nav2z::odom_tracker::CpOdomTracker::publishMessages

protected

Definition at line 146 of file cp_odom_tracker.hpp.

Referenced by CpOdomTracker(), processOdometryMessage(), and setPublishMessages().

◆ recordAngularDistanceThreshold_

double cl_nav2z::odom_tracker::CpOdomTracker::recordAngularDistanceThreshold_

protected

Meters.

Definition at line 132 of file cp_odom_tracker.hpp.

Referenced by onInitialize(), setWorkingMode(), updateConfiguration(), and updateRecordPath().

◆ recordPointDistanceThreshold_

double cl_nav2z::odom_tracker::CpOdomTracker::recordPointDistanceThreshold_

protected

How much distance there is between two points of the path.

Definition at line 129 of file cp_odom_tracker.hpp.

Referenced by onInitialize(), setWorkingMode(), updateConfiguration(), and updateRecordPath().

◆ robotBasePathPub_

rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr cl_nav2z::odom_tracker::CpOdomTracker::robotBasePathPub_

protected

Definition at line 119 of file cp_odom_tracker.hpp.

Referenced by onInitialize(), and rtPublishPaths().

◆ robotBasePathStackedPub_

rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr cl_nav2z::odom_tracker::CpOdomTracker::robotBasePathStackedPub_

protected

Definition at line 120 of file cp_odom_tracker.hpp.

Referenced by onInitialize(), and rtPublishPaths().

◆ subscribeToOdometryTopic_

bool cl_nav2z::odom_tracker::CpOdomTracker::subscribeToOdometryTopic_

protected

Definition at line 166 of file cp_odom_tracker.hpp.

Referenced by CpOdomTracker(), and onInitialize().

◆ workingMode_

WorkingMode cl_nav2z::odom_tracker::CpOdomTracker::workingMode_

protected

Definition at line 151 of file cp_odom_tracker.hpp.

Referenced by CpOdomTracker(), processOdometryMessage(), and setWorkingMode().

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

smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/odom_tracker/cp_odom_tracker.hpp
smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/odom_tracker/cp_odom_tracker.cpp

Classes

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ CpOdomTracker()

Member Function Documentation

◆ clearPath()

◆ getCurrentMotionGoal()

◆ getPath()

◆ logStateString()

◆ onInitialize()

◆ popPath()

◆ processOdometryMessage()

◆ pushPath() [1/2]

◆ pushPath() [2/2]

◆ rtPublishPaths()

◆ setCurrentMotionGoal()

◆ setCurrentPathName()

◆ setPublishMessages()

◆ setStartPoint() [1/2]

◆ setStartPoint() [2/2]

◆ setWorkingMode()

◆ updateAggregatedStackPath()

◆ updateClearPath()

◆ updateConfiguration()

◆ updateRecordPath()

Member Data Documentation

◆ aggregatedStackPathMsg_

◆ baseTrajectory_

◆ clearAngularDistanceThreshold_

◆ clearPointDistanceThreshold_

◆ currentMotionGoal_

◆ currentPathName_

◆ m_mutex_

◆ odomFrame_

◆ odomSub_

◆ odomTopicName_

◆ pathInfos_

◆ pathStack_

◆ publishMessages

◆ recordAngularDistanceThreshold_

◆ recordPointDistanceThreshold_

◆ robotBasePathPub_

◆ robotBasePathStackedPub_

◆ subscribeToOdometryTopic_

◆ workingMode_