Class CpOdomTracker

Nested Relationships

Nested Types

Inheritance Relationships

Base Type

  • public smacc2::ISmaccComponent

Class Documentation

class CpOdomTracker : public smacc2::ISmaccComponent

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”

Public 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()
inline void setOdomFrame(std::string odomFrame)

Protected 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 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_
struct PathInfo

Public Members

std::string name
std::optional<geometry_msgs::msg::PoseStamped> goalPose