SMACC2
Loading...
Searching...
No Matches
odom_tracker.hpp
Go to the documentation of this file.
1// Copyright 2021 RobosoftAI Inc.
2//
3// Licensed under the Apache License, Version 2.0 (the "License");
4// you may not use this file except in compliance with the License.
5// You may obtain a copy of the License at
6//
7// http://www.apache.org/licenses/LICENSE-2.0
8//
9// Unless required by applicable law or agreed to in writing, software
10// distributed under the License is distributed on an "AS IS" BASIS,
11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12// See the License for the specific language governing permissions and
13// limitations under the License.
14
15/*****************************************************************************************************************
16 *
17 * Authors: Pablo Inigo Blasco, Brett Aldrich
18 *
19 ******************************************************************************************************************/
20#pragma once
21
22#include <tf2/transform_datatypes.h>
23#include <smacc2/common.hpp>
24#include <smacc2/component.hpp>
25
26#include <rclcpp/rclcpp.hpp>
27
28#include <memory>
29#include <mutex>
30#include <vector>
31
32#include <geometry_msgs/msg/point.hpp>
33#include <geometry_msgs/msg/pose.hpp>
34#include <nav2_msgs/action/navigate_to_pose.hpp>
35#include <nav_msgs/msg/odometry.hpp>
36#include <nav_msgs/msg/path.hpp>
37
38#include <std_msgs/msg/header.hpp>
39
40namespace cl_nav2z
41{
42namespace odom_tracker
43{
44enum class WorkingMode : uint8_t
45{
46 RECORD_PATH = 0,
47 CLEAR_PATH = 1,
48 IDLE = 2
49};
50
54{
55public:
56 // by default, the component start in record_path mode and publishing the
57 // current path
58 OdomTracker(std::string odomtopicName = "/odom", std::string odomFrame = "odom");
59
60 // threadsafe
62 // The odom parameters is the main input of this tracker
63 virtual void processOdometryMessage(const nav_msgs::msg::Odometry::SharedPtr odom);
64
65 // ------ CONTROL COMMANDS ---------------------
66 // threadsafe
67 void setWorkingMode(WorkingMode workingMode);
68
69 // threadsafe
70 void setPublishMessages(bool value);
71
72 // threadsafe
73 void pushPath();
74
75 // threadsafe
76 void pushPath(std::string pathname);
77
78 // threadsafe
79 void popPath(int pathCount = 1, bool keepPreviousPath = false);
80
81 // threadsafe
82 void clearPath();
83
84 // threadsafe
85 void setStartPoint(const geometry_msgs::msg::PoseStamped & pose);
86
87 // threadsafe
88 void setStartPoint(const geometry_msgs::msg::Pose & pose);
89
90 // threadsafe - use only for recording mode, it is reset always a new path is pushed, popped or cleared
91 void setCurrentMotionGoal(const geometry_msgs::msg::PoseStamped & pose);
92
93 void setCurrentPathName(const std::string & currentPathName);
94
95 // threadsafe
96 std::optional<geometry_msgs::msg::PoseStamped> getCurrentMotionGoal();
97
98 // threadsafe
99 nav_msgs::msg::Path getPath();
100
101 void logStateString(bool debug = true);
102
103protected:
104 void onInitialize() override;
105
106 void updateConfiguration();
107
108 virtual void rtPublishPaths(rclcpp::Time timestamp);
109
110 // this is called when a new odom message is received in record path mode
111 virtual bool updateRecordPath(const nav_msgs::msg::Odometry & odom);
112
113 // this is called when a new odom message is received in clear path mode
114 virtual bool updateClearPath(const nav_msgs::msg::Odometry & odom);
115
117
118 // -------------- OUTPUTS ---------------------
119 rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr robotBasePathPub_;
120 rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr robotBasePathStackedPub_;
121
122 // --------------- INPUTS ------------------------
123 // optional, this class can be used directly calling the odomProcessing method
124 // without any subscriber
125 rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odomSub_;
126
127 // -------------- PARAMETERS ----------------------
130
133
136
139
140 std::string odomFrame_;
141
142 std::string odomTopicName_;
143
144 // --------------- STATE ---------------
145 // default true
147
149 nav_msgs::msg::Path baseTrajectory_;
150
152
153 std::vector<nav_msgs::msg::Path> pathStack_;
154
155 struct PathInfo
156 {
157 std::string name;
158 std::optional<geometry_msgs::msg::PoseStamped> goalPose;
159 };
160
161 std::vector<PathInfo> pathInfos_;
162
163 nav_msgs::msg::Path aggregatedStackPathMsg_;
164
165 // subscribes to topic on init if true
167
168 std::optional<geometry_msgs::msg::PoseStamped> currentMotionGoal_;
169 std::string currentPathName_;
170
171 std::mutex m_mutex_;
172};
173
179inline double p2pDistance(
180 const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2)
181{
182 double dx = (p1.x - p2.x);
183 double dy = (p1.y - p2.y);
184 double dz = (p2.z - p2.z);
185 double dist = sqrt(dx * dx + dy * dy + dz * dz);
186 return dist;
187}
188} // namespace odom_tracker
189} // namespace cl_nav2z
void setCurrentPathName(const std::string &currentPathName)
void setCurrentMotionGoal(const geometry_msgs::msg::PoseStamped &pose)
virtual void rtPublishPaths(rclcpp::Time timestamp)
double recordPointDistanceThreshold_
How much distance there is between two points of the path.
virtual bool updateClearPath(const nav_msgs::msg::Odometry &odom)
void logStateString(bool debug=true)
std::optional< geometry_msgs::msg::PoseStamped > currentMotionGoal_
rclcpp::Publisher< nav_msgs::msg::Path >::SharedPtr robotBasePathPub_
nav_msgs::msg::Path baseTrajectory_
Processed path for the mouth of the reel.
void setWorkingMode(WorkingMode workingMode)
std::vector< nav_msgs::msg::Path > pathStack_
void popPath(int pathCount=1, bool keepPreviousPath=false)
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 setStartPoint(const geometry_msgs::msg::PoseStamped &pose)
nav_msgs::msg::Path aggregatedStackPathMsg_
rclcpp::Subscription< nav_msgs::msg::Odometry >::SharedPtr odomSub_
rclcpp::Publisher< nav_msgs::msg::Path >::SharedPtr robotBasePathStackedPub_
virtual bool updateRecordPath(const nav_msgs::msg::Odometry &odom)
std::optional< geometry_msgs::msg::PoseStamped > getCurrentMotionGoal()
std::vector< PathInfo > pathInfos_
double p2pDistance(const geometry_msgs::msg::Point &p1, const geometry_msgs::msg::Point &p2)
std::optional< geometry_msgs::msg::PoseStamped > goalPose