#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", OdomTrackerStrategy strategy=OdomTrackerStrategy::ODOMETRY_SUBSCRIBER)

virtual void	processNewPose (const geometry_msgs::msg::PoseStamped &odom)
	odom callback: Updates the path - this must be called periodically for each odometry message.

virtual void	odomMessageCallback (const nav_msgs::msg::Odometry::SharedPtr odom)

virtual void	update ()

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)

void	updateParameters ()

void	setOdomFrame (std::string odomFrame)

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 geometry_msgs::msg::PoseStamped &odom)

virtual bool	updateClearPath (const geometry_msgs::msg::PoseStamped &odom)

void	updateAggregatedStackPath ()

rcl_interfaces::msg::SetParametersResult	parametersCallback (const std::vector< rclcpp::Parameter > &parameters)

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_

cl_nav2z::Pose *	robotPose_

rclcpp::TimerBase::SharedPtr	robotPoseTimer_

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_

OdomTrackerStrategy	strategy_

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

std::string	currentPathName_

rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr	param_callback_handle_

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 60 of file cp_odom_tracker.hpp.

Constructor & Destructor Documentation

◆ CpOdomTracker()

cl_nav2z::odom_tracker::CpOdomTracker::CpOdomTracker	(	std::string	odomtopicName = "/odom",
		std::string	odomFrame = "odom",
		OdomTrackerStrategy	strategy = OdomTrackerStrategy::ODOMETRY_SUBSCRIBER )

Definition at line 34 of file cp_odom_tracker.cpp.

: strategy_(strategy)
{
  workingMode_ = WorkingMode::RECORD_PATH;
  publishMessages = true;
  odomFrame_ = odomFrame;
  odomTopicName_ = odomTopicName;
}

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

Member Function Documentation

◆ clearPath()

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

Definition at line 338 of file cp_odom_tracker.cpp.

{
  RCLCPP_INFO(getLogger(), "odom_tracker m_mutex acquire to clear path");
  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::getLogger(), 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 398 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 404 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 296 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_ != nullptr && 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:

◆ odomMessageCallback()

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

virtual

odomMessageCallback()

Definition at line 617 of file cp_odom_tracker.cpp.

{
  geometry_msgs::msg::PoseStamped base_pose;
  base_pose.pose = odom->pose.pose;
  base_pose.header = odom->header;
 
  processNewPose(base_pose);
}

References processNewPose().

Referenced by onInitialize().

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 60 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_);
 
  param_callback_handle_ = getNode()->add_on_set_parameters_callback(
    std::bind(&CpOdomTracker::parametersCallback, 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));
 
  if (this->strategy_ == OdomTrackerStrategy::ODOMETRY_SUBSCRIBER)
  {
    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::odomMessageCallback, this, std::placeholders::_1));
  }
  else if (this->strategy_ == OdomTrackerStrategy::POSE_COMPONENT)
  {
    this->requiresComponent(robotPose_);
    robotPoseTimer_ = nh->create_wall_timer(
      std::chrono::milliseconds(100), std::bind(&CpOdomTracker::update, this));
  }
  else
  {
    RCLCPP_ERROR_STREAM(
      nh->get_logger(), "[CpOdomTracker] unknown strategy: " << (int)this->strategy_);
  }
}

References clearAngularDistanceThreshold_, clearPointDistanceThreshold_, smacc2::ISmaccComponent::getLogger(), smacc2::ISmaccComponent::getNode(), cl_nav2z::odom_tracker::ODOMETRY_SUBSCRIBER, odomFrame_, odomMessageCallback(), odomSub_, odomTopicName_, param_callback_handle_, cl_nav2z::odom_tracker::parameterDeclareAndtryGetOrSet(), parametersCallback(), cl_nav2z::odom_tracker::POSE_COMPONENT, recordAngularDistanceThreshold_, recordPointDistanceThreshold_, smacc2::ISmaccComponent::requiresComponent(), robotBasePathPub_, robotBasePathStackedPub_, robotPose_, robotPoseTimer_, strategy_, and update().

Here is the call graph for this function:

◆ parametersCallback()

rcl_interfaces::msg::SetParametersResult cl_nav2z::odom_tracker::CpOdomTracker::parametersCallback ( const std::vector< rclcpp::Parameter > & parameters )

protected

parametersCallback()

Definition at line 118 of file cp_odom_tracker.cpp.

{
  std::lock_guard<std::mutex> lock(m_mutex_);
  RCLCPP_INFO(getLogger(), "odom_tracker m_mutex acquire");
 
  for (const auto & param : parameters)
  {
    if (param.get_name() == "record_point_distance_threshold")
    {
      recordPointDistanceThreshold_ = param.as_double();
    }
    else if (param.get_name() == "record_angular_distance_threshold")
    {
      recordAngularDistanceThreshold_ = param.as_double();
    }
    else if (param.get_name() == "clear_point_distance_threshold")
    {
      clearPointDistanceThreshold_ = param.as_double();
    }
    else if (param.get_name() == "clear_angular_distance_threshold")
    {
      clearAngularDistanceThreshold_ = param.as_double();
    }
    else if (param.get_name() == "odom_frame")
    {
      odomFrame_ = param.as_string();
    }
  }
 
  rcl_interfaces::msg::SetParametersResult result;
  result.successful = true;
  result.reason = "success";
  return result;
}

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

Referenced by onInitialize().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ popPath()

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

Definition at line 251 of file cp_odom_tracker.cpp.

{
  RCLCPP_INFO(getLogger(), "odom_tracker m_mutex acquire to pop path");
  std::lock_guard<std::mutex> lock(m_mutex_);
 
  RCLCPP_INFO(getLogger(), "POP PATH ENTRY");
  this->logStateString();
 
  if (this->odomSub_ != nullptr && 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:

◆ processNewPose()

void cl_nav2z::odom_tracker::CpOdomTracker::processNewPose ( const geometry_msgs::msg::PoseStamped & odom )

virtual

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

processNewPose()

Definition at line 631 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 odomMessageCallback(), and update().

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 213 of file cp_odom_tracker.cpp.

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

cl_nav2z::odom_tracker::CpOdomTracker::pushPath

void pushPath()

Definition cp_odom_tracker.cpp:213

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 215 of file cp_odom_tracker.cpp.

{
  RCLCPP_INFO(getLogger(), "odom_tracker m_mutex acquire, to push path");
  std::lock_guard<std::mutex> lock(m_mutex_);
 
  this->logStateString(false);
  if (this->odomSub_ != nullptr && 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 415 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 processNewPose().

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 392 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 351 of file cp_odom_tracker.cpp.

{
  RCLCPP_INFO_STREAM(getLogger(), "[CpOdomTracker] set current path name: " << currentPathName);
  currentPathName_ = currentPathName;
}

References currentPathName_, and smacc2::ISmaccComponent::getLogger().

Here is the call graph for this function:

◆ setOdomFrame()

void cl_nav2z::odom_tracker::CpOdomTracker::setOdomFrame ( std::string odomFrame )

inline

Definition at line 119 of file cp_odom_tracker.hpp.

  {
    odomFrame_ = odomFrame;
    getNode()->set_parameter(rclcpp::Parameter("odom_frame", odomFrame));
  }

References smacc2::ISmaccComponent::getNode(), and odomFrame_.

Here is the call graph for this function:

◆ setPublishMessages()

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

setPublishMessages()

Definition at line 204 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 372 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 357 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 159 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_ != nullptr && 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:

◆ update()

void cl_nav2z::odom_tracker::CpOdomTracker::update ( )

virtual

update()

Definition at line 603 of file cp_odom_tracker.cpp.

{
  RCLCPP_INFO(getLogger(), "odom_tracker update");
  auto pose = robotPose_->toPoseStampedMsg();
  RCLCPP_INFO(getLogger(), "odom tracker updatd pose, frame: %s", pose.header.frame_id.c_str());
  processNewPose(pose);
  RCLCPP_INFO(getLogger(), "odom_tracker update end");
}

References smacc2::ISmaccComponent::getLogger(), processNewPose(), robotPose_, and cl_nav2z::Pose::toPoseStampedMsg().

Referenced by onInitialize().

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 424 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 geometry_msgs::msg::PoseStamped & base_pose )

protectedvirtual

updateBackward()

Track robot base pose

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

Definition at line 441 of file cp_odom_tracker.cpp.

{
  // we initially accept any message if the queue is empty
  baseTrajectory_.header = base_pose.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 processNewPose().

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 572 of file cp_odom_tracker.cpp.

{
  return;
 
  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 processNewPose().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ updateParameters()

void cl_nav2z::odom_tracker::CpOdomTracker::updateParameters ( )

◆ updateRecordPath()

bool cl_nav2z::odom_tracker::CpOdomTracker::updateRecordPath ( const geometry_msgs::msg::PoseStamped & base_pose )

protectedvirtual

updateRecordPath()

Track robot base pose

Definition at line 517 of file cp_odom_tracker.cpp.

{
  baseTrajectory_.header = base_pose.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 processNewPose().

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 188 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 174 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 163 of file cp_odom_tracker.hpp.

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

◆ clearPointDistanceThreshold_

double cl_nav2z::odom_tracker::CpOdomTracker::clearPointDistanceThreshold_

protected

rads

Definition at line 160 of file cp_odom_tracker.hpp.

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

◆ currentMotionGoal_

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

protected

Definition at line 192 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 193 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 200 of file cp_odom_tracker.hpp.

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

◆ odomFrame_

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

protected

Definition at line 165 of file cp_odom_tracker.hpp.

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

◆ odomSub_

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

protected

Definition at line 147 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 167 of file cp_odom_tracker.hpp.

Referenced by CpOdomTracker(), and onInitialize().

◆ param_callback_handle_

rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr cl_nav2z::odom_tracker::CpOdomTracker::param_callback_handle_

protected

Definition at line 198 of file cp_odom_tracker.hpp.

Referenced by onInitialize().

◆ pathInfos_

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

protected

Definition at line 186 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 178 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 171 of file cp_odom_tracker.hpp.

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

◆ recordAngularDistanceThreshold_

double cl_nav2z::odom_tracker::CpOdomTracker::recordAngularDistanceThreshold_

protected

Meters.

Definition at line 157 of file cp_odom_tracker.hpp.

Referenced by onInitialize(), parametersCallback(), 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 154 of file cp_odom_tracker.hpp.

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

◆ robotBasePathPub_

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

protected

Definition at line 141 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 142 of file cp_odom_tracker.hpp.

Referenced by onInitialize(), and rtPublishPaths().

◆ robotPose_

cl_nav2z::Pose* cl_nav2z::odom_tracker::CpOdomTracker::robotPose_

protected

Definition at line 149 of file cp_odom_tracker.hpp.

Referenced by onInitialize(), and update().

◆ robotPoseTimer_

rclcpp::TimerBase::SharedPtr cl_nav2z::odom_tracker::CpOdomTracker::robotPoseTimer_

protected

Definition at line 150 of file cp_odom_tracker.hpp.

Referenced by onInitialize().

◆ strategy_

OdomTrackerStrategy cl_nav2z::odom_tracker::CpOdomTracker::strategy_

protected

Definition at line 190 of file cp_odom_tracker.hpp.

Referenced by onInitialize().

◆ workingMode_

WorkingMode cl_nav2z::odom_tracker::CpOdomTracker::workingMode_

protected

Definition at line 176 of file cp_odom_tracker.hpp.

Referenced by CpOdomTracker(), processNewPose(), 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()

◆ odomMessageCallback()

◆ onInitialize()

◆ parametersCallback()

◆ popPath()

◆ processNewPose()

◆ pushPath() [1/2]

◆ pushPath() [2/2]

◆ rtPublishPaths()

◆ setCurrentMotionGoal()

◆ setCurrentPathName()

◆ setOdomFrame()

◆ setPublishMessages()

◆ setStartPoint() [1/2]

◆ setStartPoint() [2/2]

◆ setWorkingMode()

◆ update()

◆ updateAggregatedStackPath()

◆ updateClearPath()

◆ updateConfiguration()

◆ updateParameters()

◆ updateRecordPath()

Member Data Documentation

◆ aggregatedStackPathMsg_

◆ baseTrajectory_

◆ clearAngularDistanceThreshold_

◆ clearPointDistanceThreshold_

◆ currentMotionGoal_

◆ currentPathName_

◆ m_mutex_

◆ odomFrame_

◆ odomSub_

◆ odomTopicName_

◆ param_callback_handle_

◆ pathInfos_

◆ pathStack_

◆ publishMessages

◆ recordAngularDistanceThreshold_

◆ recordPointDistanceThreshold_

◆ robotBasePathPub_

◆ robotBasePathStackedPub_

◆ robotPose_

◆ robotPoseTimer_

◆ strategy_

◆ workingMode_