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 void OnActivateWithGoal(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 onDeactivate() override {}
 
  void onActivate() override;
  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)
{
  (void)goal_ptr;
}
 
template <class ActionT>
void ModeBase<ActionT>::onActivate()
{
  OnActivateWithGoal(action_context_ptr_->getGoalHandlePtr()->get_goal());
}
 
template <class ActionT>
void ModeBase<ActionT>::updateSetpoint(float dt_s)
{
  UpdateSetpointWithGoal(
    dt_s, action_context_ptr_->getGoalHandlePtr()->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