cp_odom_tracker.hpp

Go to the documentation of this file.

// Copyright 2021 RobosoftAI Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
/*****************************************************************************************************************
 *
 *   Authors: Pablo Inigo Blasco, Brett Aldrich
 *
 ******************************************************************************************************************/
#pragma once
 
#include <tf2/transform_datatypes.h>
#include <smacc2/common.hpp>
#include <smacc2/component.hpp>
 
#include <rclcpp/rclcpp.hpp>
 
#include <memory>
#include <mutex>
#include <vector>
 
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <nav2_msgs/action/navigate_to_pose.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <nav_msgs/msg/path.hpp>
 
#include <nav2z_client/components/pose/cp_pose.hpp>
#include <std_msgs/msg/header.hpp>
 
namespace cl_nav2z
{
namespace odom_tracker
{
enum class WorkingMode : uint8_t
{
  RECORD_PATH = 0,
  CLEAR_PATH = 1,
  IDLE = 2
};
 
enum class OdomTrackerStrategy
{
  ODOMETRY_SUBSCRIBER,
  POSE_COMPONENT
};
 
class CpOdomTracker : public smacc2::ISmaccComponent
{
public:
  // by default, the component start in record_path mode and publishing the
  // current path
  CpOdomTracker(
    std::string odomtopicName = "/odom", std::string odomFrame = "odom",
    OdomTrackerStrategy strategy = OdomTrackerStrategy::ODOMETRY_SUBSCRIBER);
 
  // threadsafe
  // The odom parameters is the main input of this tracker
  virtual void processNewPose(const geometry_msgs::msg::PoseStamped & odom);
 
  virtual void odomMessageCallback(const nav_msgs::msg::Odometry::SharedPtr odom);
 
  virtual void update();
 
  // ------ CONTROL COMMANDS ---------------------
  // threadsafe
  void setWorkingMode(WorkingMode workingMode);
 
  // threadsafe
  void setPublishMessages(bool value);
 
  // threadsafe
  void pushPath();
 
  // threadsafe
  void pushPath(std::string pathname);
 
  // threadsafe
  void popPath(int pathCount = 1, bool keepPreviousPath = false);
 
  // threadsafe
  void clearPath();
 
  // threadsafe
  void setStartPoint(const geometry_msgs::msg::PoseStamped & pose);
 
  // threadsafe
  void setStartPoint(const geometry_msgs::msg::Pose & pose);
 
  // threadsafe - use only for recording mode, it is reset always a new path is pushed, popped or cleared
  void setCurrentMotionGoal(const geometry_msgs::msg::PoseStamped & pose);
 
  void setCurrentPathName(const std::string & currentPathName);
 
  // threadsafe
  std::optional<geometry_msgs::msg::PoseStamped> getCurrentMotionGoal();
 
  // threadsafe
  nav_msgs::msg::Path getPath();
 
  void logStateString(bool debug = true);
 
  // parameters update callback
  void updateParameters();
 
  inline void setOdomFrame(std::string odomFrame)
  {
    odomFrame_ = odomFrame;
    getNode()->set_parameter(rclcpp::Parameter("odom_frame", odomFrame));
  }
 
protected:
  void onInitialize() override;
 
  void updateConfiguration();
 
  virtual void rtPublishPaths(rclcpp::Time timestamp);
 
  // this is called when a new odom message is received in record path mode
  virtual bool updateRecordPath(const geometry_msgs::msg::PoseStamped & odom);
 
  // this is called when a new odom message is received in clear path mode
  virtual bool updateClearPath(const geometry_msgs::msg::PoseStamped & odom);
 
  void updateAggregatedStackPath();
 
  // -------------- OUTPUTS ---------------------
  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr robotBasePathPub_;
  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr robotBasePathStackedPub_;
 
  // --------------- INPUTS ------------------------
  // optional, this class can be used directly calling the odomProcessing method
  // without any subscriber
  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odomSub_;
 
  cl_nav2z::Pose * robotPose_;
  rclcpp::TimerBase::SharedPtr robotPoseTimer_;
 
  // -------------- PARAMETERS ----------------------
  double recordPointDistanceThreshold_;
 
  double recordAngularDistanceThreshold_;
 
  double clearPointDistanceThreshold_;
 
  double clearAngularDistanceThreshold_;
 
  std::string odomFrame_;
 
  std::string odomTopicName_;
 
  // --------------- STATE ---------------
  // default true
  bool publishMessages;
 
  nav_msgs::msg::Path baseTrajectory_;
 
  WorkingMode workingMode_;
 
  std::vector<nav_msgs::msg::Path> pathStack_;
 
  struct PathInfo
  {
    std::string name;
    std::optional<geometry_msgs::msg::PoseStamped> goalPose;
  };
 
  std::vector<PathInfo> pathInfos_;
 
  nav_msgs::msg::Path aggregatedStackPathMsg_;
 
  OdomTrackerStrategy strategy_;
 
  std::optional<geometry_msgs::msg::PoseStamped> currentMotionGoal_;
  std::string currentPathName_;
 
  rcl_interfaces::msg::SetParametersResult parametersCallback(
    const std::vector<rclcpp::Parameter> & parameters);
 
  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_;
 
  std::mutex m_mutex_;
};
 
inline double p2pDistance(
  const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2)
{
  double dx = (p1.x - p2.x);
  double dy = (p1.y - p2.y);
  double dz = (p2.z - p2.z);
  double dist = sqrt(dx * dx + dy * dy + dz * dz);
  return dist;
}
}  // namespace odom_tracker
}  // namespace cl_nav2z