mode.hpp

// Copyright 2024 Robin Müller
//
// 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.
 
#pragma once
 
#include <Eigen/Geometry>
 
#include "auto_apms_util/action_context.hpp"
#include "px4_ros2/components/mode.hpp"
#include "px4_ros2/odometry/attitude.hpp"
#include "px4_ros2/odometry/global_position.hpp"
#include "px4_ros2/odometry/local_position.hpp"
#include "px4_ros2/utils/geodesic.hpp"
#include "px4_ros2/utils/geometry.hpp"
 
namespace auto_apms_px4
{

template <class ActionT>
class ModeBase : public px4_ros2::ModeBase
{
protected:
  using ActionContextType = auto_apms_util::ActionContext<ActionT>;
  using Goal = typename ActionContextType::Goal;
  using Result = typename ActionContextType::Result;
  using Feedback = typename ActionContextType::Feedback;
 
  ModeBase(rclcpp::Node & node, Settings settings, std::shared_ptr<ActionContextType> action_context_ptr)
  : px4_ros2::ModeBase{node, settings}, action_context_ptr_{action_context_ptr}
  {
  }
 
private:
  /* Virtual methods */
 
  virtual void onActivateWithGoal(std::shared_ptr<const Goal> goal_ptr);
  virtual void onDeactivateWithGoal(std::shared_ptr<const Goal> goal_ptr);

 
  virtual void updateSetpointWithGoal(
    float dt_s, std::shared_ptr<const Goal> goal_ptr, std::shared_ptr<Feedback> feedback_ptr,
    std::shared_ptr<Result> result_ptr) = 0;
  virtual void updateSetpointOnCancel(
    float dt_s, std::shared_ptr<const Goal> goal_ptr, std::shared_ptr<Feedback> feedback_ptr,
    std::shared_ptr<Result> result_ptr);
 
  /* Overridden base methods */
 
  void onDeactivate() override final;
  void onActivate() override final;
  void updateSetpoint(float dt_s) override;
 
  const std::shared_ptr<ActionContextType> action_context_ptr_;
};
 
template <class ActionT>
void ModeBase<ActionT>::onActivateWithGoal(std::shared_ptr<const Goal> /*goal_ptr*/)
{
}
 
template <class ActionT>
void ModeBase<ActionT>::onDeactivateWithGoal(std::shared_ptr<const Goal> /*goal_ptr*/)
{
}
 
template <class ActionT>
void ModeBase<ActionT>::updateSetpointOnCancel(
  float /*dt_s*/, std::shared_ptr<const Goal> /*goal_ptr*/, std::shared_ptr<Feedback> /*feedback_ptr*/,
  std::shared_ptr<Result> /*result_ptr*/)
{
  completed(px4_ros2::Result::Interrupted);
}
 
template <class ActionT>
void ModeBase<ActionT>::onActivate()
{
  onActivateWithGoal(action_context_ptr_->getGoalHandlePtr()->get_goal());
}
 
template <class ActionT>
void ModeBase<ActionT>::onDeactivate()
{
  const auto goal_handle_ptr = action_context_ptr_->getGoalHandlePtr();
  onDeactivateWithGoal(goal_handle_ptr ? goal_handle_ptr->get_goal() : nullptr);
}
 
template <class ActionT>
void ModeBase<ActionT>::updateSetpoint(float dt_s)
{
  if (action_context_ptr_->isValid()) {
    const auto goal_handle_ptr = action_context_ptr_->getGoalHandlePtr();
    if (goal_handle_ptr->is_canceling()) {
      updateSetpointOnCancel(
        dt_s, goal_handle_ptr->get_goal(), action_context_ptr_->getFeedbackPtr(), action_context_ptr_->getResultPtr());
    } else {
      updateSetpointWithGoal(
        dt_s, goal_handle_ptr->get_goal(), action_context_ptr_->getFeedbackPtr(), action_context_ptr_->getResultPtr());
    }
  }
}
 
template <class ActionT>
class PositionAwareMode : public ModeBase<ActionT>
{
protected:
  using typename ModeBase<ActionT>::ActionContextType;
 
  PositionAwareMode(
    rclcpp::Node & node, px4_ros2::ModeBase::Settings settings, std::shared_ptr<ActionContextType> action_context_ptr)
  : ModeBase<ActionT>{node, settings, action_context_ptr}
  {
    vehicle_global_position_ptr_ = std::make_shared<px4_ros2::OdometryGlobalPosition>(*this);
    vehicle_local_position_ptr_ = std::make_shared<px4_ros2::OdometryLocalPosition>(*this);
    vehicle_attitude_ptr_ = std::make_shared<px4_ros2::OdometryAttitude>(*this);
  }
 
  bool isGlobalPositionReached(
    const Eigen::Vector3d & target_position_f_glob, double reached_thresh_pos_m = 0.5,
    double reached_thresh_vel_m_s = 0.3) const;
 
  bool isLocalPositionReached(
    const Eigen::Vector3d & target_position_f_ned, double reached_thresh_pos_m = 0.5,
    double reached_thresh_vel_m_s = 0.3) const;
 
  bool isGlobalAltitudeReached(
    float target_altitude_amsl_m, double reached_thresh_pos_m = 0.5, double reached_thresh_vel_m_s = 0.3) const;
 
  bool isLocalAltitudeReached(
    float target_altitude_hagl_m, double reached_thresh_pos_m = 0.5, double reached_thresh_vel_m_s = 0.3) const;
 
  bool isHeadingReached(float target_heading_rad, double reached_thresh_heading_rad = 0.12) const;
 
protected:
  std::shared_ptr<px4_ros2::OdometryGlobalPosition> vehicle_global_position_ptr_;
  std::shared_ptr<px4_ros2::OdometryLocalPosition> vehicle_local_position_ptr_;
  std::shared_ptr<px4_ros2::OdometryAttitude> vehicle_attitude_ptr_;
};
 
template <class ActionT>
bool PositionAwareMode<ActionT>::isGlobalPositionReached(
  const Eigen::Vector3d & target_position_f_glob, double reached_thresh_pos_m, double reached_thresh_vel_m_s) const
{
  const float position_error_m =
    px4_ros2::distanceToGlobalPosition(vehicle_global_position_ptr_->position(), target_position_f_glob);
  return (position_error_m < reached_thresh_pos_m) &&
         (vehicle_local_position_ptr_->velocityNed().norm() <= reached_thresh_vel_m_s);
}
 
template <class ActionT>
bool PositionAwareMode<ActionT>::isLocalPositionReached(
  const Eigen::Vector3d & target_position_f_ned, double reached_thresh_pos_m, double reached_thresh_vel_m_s) const
{
  const px4_msgs::msg::VehicleLocalPosition & pos = vehicle_local_position_ptr_->last();
  const Eigen::Vector3d vec(pos.x, pos.y, pos.z);
  const double position_error_m = (target_position_f_ned - vec).norm();
  return (position_error_m < reached_thresh_pos_m) &&
         (vehicle_local_position_ptr_->velocityNed().norm() <= reached_thresh_vel_m_s);
}
 
template <class ActionT>
bool PositionAwareMode<ActionT>::isGlobalAltitudeReached(
  float target_altitude_amsl_m, double reached_thresh_pos_m, double reached_thresh_vel_m_s) const
{
  Eigen::Vector3d target_position_f_glob = vehicle_global_position_ptr_->position();
  target_position_f_glob.z() = target_altitude_amsl_m;
  return isGlobalPositionReached(target_position_f_glob, reached_thresh_pos_m, reached_thresh_vel_m_s);
}
 
template <class ActionT>
bool PositionAwareMode<ActionT>::isLocalAltitudeReached(
  float target_altitude_hagl_m, double reached_thresh_pos_m, double reached_thresh_vel_m_s) const
{
  const px4_msgs::msg::VehicleLocalPosition & pos = vehicle_local_position_ptr_->last();
  const Eigen::Vector3d target_position_f_ned(pos.x, pos.y, target_altitude_hagl_m);
  return isLocalPositionReached(target_position_f_ned, reached_thresh_pos_m, reached_thresh_vel_m_s);
}
 
template <class ActionT>
bool PositionAwareMode<ActionT>::isHeadingReached(float target_heading_rad, double reached_thresh_heading_rad) const
{
  const float heading_error_wrapped = px4_ros2::wrapPi(target_heading_rad - vehicle_attitude_ptr_->yaw());
  return fabsf(heading_error_wrapped) <= fabsf(reached_thresh_heading_rad);
}
 
}  // namespace auto_apms_px4