cl_nav2z::odom_tracker::CpOdomTracker Class Reference

#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)
	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)
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 TOrthogonal , typename TClient >
void	onComponentInitialization ()
template<typename EventType >
void	postEvent (const EventType &ev)
template<typename EventType >
void	postEvent ()
template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()
template<typename TOrthogonal , typename TSourceObject >
void	onStateOrthogonalAllocation ()
template<typename TComponent >
void	requiresComponent (TComponent *&requiredComponentStorage, bool throwExceptionIfNotExist)
template<typename TComponent >
void	requiresComponent (std::string name, TComponent *&requiredComponentStorage, bool throwExceptionIfNotExist)
template<typename TComponent >
void	requiresComponent (TComponent *&requiredComponentStorage, ComponentRequirement requirementType=ComponentRequirement::SOFT)
template<typename TComponent >
void	requiresComponent (std::string name, TComponent *&requiredComponentStorage, ComponentRequirement requirementType=ComponentRequirement::SOFT)
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::CpPose *	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_
cl_nav2z::Pose *	robotPose_
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() [1/2]

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

Definition at line 36 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_.

CpOdomTracker() [2/2]

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

Member Function Documentation

clearPath() [1/2]

void cl_nav2z::odom_tracker::CpOdomTracker::clearPath

(

)

Definition at line 340 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(), cl_nav2z::CbTrackPathOdometry::onEntry(), and cl_nav2z::CbTrackPathSLAM::onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

clearPath() [2/2]

void cl_nav2z::odom_tracker::CpOdomTracker::clearPath

(

)

getCurrentMotionGoal() [1/2]

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

(

)

Definition at line 400 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:

getCurrentMotionGoal() [2/2]

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

(

)

getPath() [1/2]

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

(

)

Definition at line 406 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:

getPath() [2/2]

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

(

)

logStateString() [1/2]

void cl_nav2z::odom_tracker::CpOdomTracker::logStateString

(

bool

debug = true

)

Definition at line 298 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:

logStateString() [2/2]

void cl_nav2z::odom_tracker::CpOdomTracker::logStateString

(

bool

debug = true

)

odomMessageCallback() [1/2]

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

virtual

odomMessageCallback()

Definition at line 619 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:

odomMessageCallback() [2/2]

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

virtual

onInitialize() [1/2]

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

overrideprotectedvirtual

Reimplemented from smacc2::ISmaccComponent.

Definition at line 62 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_, ComponentRequirement::HARD);
    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:

onInitialize() [2/2]

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

overrideprotectedvirtual

Reimplemented from smacc2::ISmaccComponent.

parametersCallback() [1/2]

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

protected

parametersCallback()

Definition at line 120 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:

parametersCallback() [2/2]

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

protected

popPath() [1/2]

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

Definition at line 253 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:

popPath() [2/2]

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

processNewPose() [1/2]

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 633 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:

processNewPose() [2/2]

virtual 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.

pushPath() [1/4]

void cl_nav2z::odom_tracker::CpOdomTracker::pushPath

(

)

Definition at line 215 of file cp_odom_tracker.cpp.

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

References pushPath().

Referenced by CpOdomTrackerActionServer::execute(), cl_nav2z::CbAbsoluteRotate::onEntry(), cl_nav2z::CbNavigateForward::onEntry(), cl_nav2z::CbNavigateGlobalPosition::onEntry(), cl_nav2z::CbRotate::onEntry(), pushPath(), and cl_nav2z::CpWaypointNavigator::sendNextGoal().

Here is the call graph for this function:

Here is the caller graph for this function:

pushPath() [2/4]

void cl_nav2z::odom_tracker::CpOdomTracker::pushPath

(

)

pushPath() [3/4]

void cl_nav2z::odom_tracker::CpOdomTracker::pushPath

(

std::string

pathname

)

Definition at line 217 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:

pushPath() [4/4]

void cl_nav2z::odom_tracker::CpOdomTracker::pushPath

(

std::string

pathname

)

rtPublishPaths() [1/2]

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

protectedvirtual

rtPublishPaths()

Definition at line 417 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:

rtPublishPaths() [2/2]

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

protectedvirtual

setCurrentMotionGoal() [1/2]

void cl_nav2z::odom_tracker::CpOdomTracker::setCurrentMotionGoal

(

const geometry_msgs::msg::PoseStamped &

pose

)

Definition at line 394 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::CbAbsoluteRotate::onEntry(), and cl_nav2z::CbNavigateForward::onEntry().

Here is the caller graph for this function:

setCurrentMotionGoal() [2/2]

void cl_nav2z::odom_tracker::CpOdomTracker::setCurrentMotionGoal

(

const geometry_msgs::msg::PoseStamped &

pose

)

setCurrentPathName() [1/2]

void cl_nav2z::odom_tracker::CpOdomTracker::setCurrentPathName

(

const std::string &

currentPathName

)

Definition at line 353 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:

setCurrentPathName() [2/2]

void cl_nav2z::odom_tracker::CpOdomTracker::setCurrentPathName

(

const std::string &

currentPathName

)

setOdomFrame() [1/2]

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:

setOdomFrame() [2/2]

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() [1/2]

void cl_nav2z::odom_tracker::CpOdomTracker::setPublishMessages

(

bool

value

)

setPublishMessages()

Definition at line 206 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:

setPublishMessages() [2/2]

void cl_nav2z::odom_tracker::CpOdomTracker::setPublishMessages

(

bool

value

)

setStartPoint() [1/4]

void cl_nav2z::odom_tracker::CpOdomTracker::setStartPoint

(

const geometry_msgs::msg::Pose &

pose

)

Definition at line 374 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/4]

void cl_nav2z::odom_tracker::CpOdomTracker::setStartPoint

(

const geometry_msgs::msg::Pose &

pose

)

setStartPoint() [3/4]

void cl_nav2z::odom_tracker::CpOdomTracker::setStartPoint

(

const geometry_msgs::msg::PoseStamped &

pose

)

Definition at line 359 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::CbAbsoluteRotate::onEntry(), cl_nav2z::CbNavigateBackwards::onEntry(), cl_nav2z::CbNavigateForward::onEntry(), cl_nav2z::CbNavigateGlobalPosition::onEntry(), cl_nav2z::CbRotate::onEntry(), and cl_nav2z::CpWaypointNavigator::sendNextGoal().

Here is the call graph for this function:

Here is the caller graph for this function:

setStartPoint() [4/4]

void cl_nav2z::odom_tracker::CpOdomTracker::setStartPoint

(

const geometry_msgs::msg::PoseStamped &

pose

)

setWorkingMode() [1/2]

void cl_nav2z::odom_tracker::CpOdomTracker::setWorkingMode

(

WorkingMode

workingMode

)

setWorkingMode()

Definition at line 161 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::CbAbsoluteRotate::onEntry(), cl_nav2z::CbNavigateBackwards::onEntry(), cl_nav2z::CbNavigateForward::onEntry(), cl_nav2z::CbNavigateGlobalPosition::onEntry(), cl_nav2z::CbRotate::onEntry(), cl_nav2z::CbUndoPathBackwards::onEntry(), cl_nav2z::CbNavigateBackwards::onExit(), cl_nav2z::CbNavigateForward::onExit(), and cl_nav2z::CpWaypointNavigator::sendNextGoal().

Here is the call graph for this function:

Here is the caller graph for this function:

setWorkingMode() [2/2]

void cl_nav2z::odom_tracker::CpOdomTracker::setWorkingMode

(

WorkingMode

workingMode

)

update() [1/2]

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

virtual

update()

Definition at line 605 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::CpPose::toPoseStampedMsg().

Referenced by onInitialize().

Here is the call graph for this function:

Here is the caller graph for this function:

update() [2/2]

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

virtual

updateAggregatedStackPath() [1/2]

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

protected

Definition at line 426 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:

updateAggregatedStackPath() [2/2]

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

protected

updateClearPath() [1/2]

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

Track robot base pose

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

Definition at line 443 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:

updateClearPath() [2/2]

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

protectedvirtual

updateConfiguration() [1/2]

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

protected

reconfigCB()

Definition at line 574 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:

updateConfiguration() [2/2]

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

protected

updateParameters() [1/2]

void cl_nav2z::odom_tracker::CpOdomTracker::updateParameters

(

)

updateParameters() [2/2]

void cl_nav2z::odom_tracker::CpOdomTracker::updateParameters

(

)

updateRecordPath() [1/2]

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

protectedvirtual

updateRecordPath()

Track robot base pose

Track robot base pose

Definition at line 519 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:

updateRecordPath() [2/2]

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

protectedvirtual

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_ [1/2]

cl_nav2z::CpPose* cl_nav2z::odom_tracker::CpOdomTracker::robotPose_

protected

Definition at line 149 of file cp_odom_tracker.hpp.

Referenced by onInitialize(), and update().

robotPose_ [2/2]

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

protected

Definition at line 149 of file cp_odom_tracker.hpp.

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/cl_nav2z/include/cl_nav2z/components/odom_tracker/cp_odom_tracker.hpp
• smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/odom_tracker/cp_odom_tracker.hpp
• smacc2_client_library/cl_nav2z/src/cl_nav2z/components/odom_tracker/cp_odom_tracker.cpp
• smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/odom_tracker/cp_odom_tracker.cpp